--- /dev/null
+resources:
+ containers:
+ - container: fmt
+ image: pointcloudlibrary/fmt
+ - container: env1604
+ image: pointcloudlibrary/env:16.04
+ - container: env2004
+ image: pointcloudlibrary/env:20.04
+ - container: doc
+ image: pointcloudlibrary/doc
+
+stages:
+ - stage: formatting
+ displayName: Formatting
+ jobs:
+ - template: formatting.yaml
+ - stage: build_ubuntu
+ displayName: Build Ubuntu
+ dependsOn: formatting
+ jobs:
+ - template: build-ubuntu.yaml
+ - stage: build_osx
+ displayName: Build macOS
+ dependsOn: formatting
+ jobs:
+ - template: build-macos.yaml
+ - stage: build_windows
+ displayName: Build Windows
+ dependsOn: formatting
+ jobs:
+ - template: build-windows.yaml
+ - stage: documentation
+ displayName: Documentation
+ dependsOn: []
+ jobs:
+ - template: documentation.yaml
+ - stage: tutorials
+ displayName: Tutorials
+ dependsOn: build_ubuntu
+ jobs:
+ - template: tutorials.yaml
+
--- /dev/null
+jobs:
+ - job: osx
+ displayName: macOS
+ pool:
+ vmImage: '$(VMIMAGE)'
+ strategy:
+ matrix:
+ Catalina 10.15:
+ VMIMAGE: 'macOS-10.15'
+ OSX_VERSION: '10.15'
+ Mojave 10.14:
+ VMIMAGE: 'macOS-10.14'
+ OSX_VERSION: '10.14'
+ timeoutInMinutes: 0
+ variables:
+ BUILD_DIR: '$(Agent.WorkFolder)/build'
+ GOOGLE_TEST_DIR: '$(Agent.WorkFolder)/googletest'
+ CMAKE_CXX_FLAGS: '-Wall -Wextra -Werror -Wabi'
+ steps:
+ - checkout: self
+ # find the commit hash on a quick non-forced update too
+ fetchDepth: 10
+ - script: |
+ brew install pkg-config qt5 libpcap brewsci/science/openni libomp
+ brew install vtk --with-qt --without-python@2
+ brew install --only-dependencies pcl
+ git clone https://github.com/abseil/googletest.git $GOOGLE_TEST_DIR # the official endpoint changed to abseil/googletest
+ cd $GOOGLE_TEST_DIR && git checkout release-1.8.1
+ displayName: 'Install Dependencies'
+ - script: |
+ mkdir $BUILD_DIR && cd $BUILD_DIR
+ # Surface switched off due to OpenGL deprecation on Mac
+ cmake $(Build.SourcesDirectory) \
+ -DCMAKE_BUILD_TYPE="Release" \
+ -DCMAKE_OSX_SYSROOT="/Library/Developer/CommandLineTools/SDKs/MacOSX$(OSX_VERSION).sdk" \
+ -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \
+ -DGTEST_SRC_DIR="$GOOGLE_TEST_DIR/googletest" \
+ -DGTEST_INCLUDE_DIR="$GOOGLE_TEST_DIR/googletest/include" \
+ -DQt5_DIR=/usr/local/opt/qt5/lib/cmake/Qt5 \
+ -DPCL_ONLY_CORE_POINT_TYPES=ON \
+ -DBUILD_simulation=ON \
+ -DBUILD_surface=OFF \
+ -DBUILD_global_tests=ON \
+ -DBUILD_examples=ON \
+ -DBUILD_tools=ON \
+ -DBUILD_apps=ON \
+ -DBUILD_apps_3d_rec_framework=ON \
+ -DBUILD_apps_cloud_composer=ON \
+ -DBUILD_apps_in_hand_scanner=ON \
+ -DBUILD_apps_modeler=ON \
+ -DBUILD_apps_point_cloud_editor=ON
+ displayName: 'CMake Configuration'
+ - script: |
+ cd $BUILD_DIR
+ cmake --build . -- -j2
+ displayName: 'Build Library'
+ - script: cd $BUILD_DIR && cmake --build . -- tests
+ displayName: 'Run Unit Tests'
+ - task: PublishTestResults@2
+ inputs:
+ testResultsFormat: 'CTest'
+ testResultsFiles: '**/Test*.xml'
+ searchFolder: '$(Agent.WorkFolder)/build'
+ condition: succeededOrFailed()
+
+++ /dev/null
-jobs:
- - job: osx
- strategy:
- matrix:
- macOS Catalina:
- VMIMAGE: 'macOS-10.15'
- macOS Mojave:
- VMIMAGE: 'macOS-10.14'
- timeoutInMinutes: 0
- pool:
- vmImage: '$(VMIMAGE)'
- variables:
- BUILD_DIR: '$(Agent.WorkFolder)/build'
- GOOGLE_TEST_DIR: '$(Agent.WorkFolder)/googletest'
- CMAKE_CXX_FLAGS: '-Wall -Wextra -Wabi -O2'
- steps:
- - script: |
- brew install pkg-config qt5 libpcap brewsci/science/openni
- brew install vtk --with-qt --without-python@2
- brew install --only-dependencies pcl
- git clone https://github.com/abseil/googletest.git $GOOGLE_TEST_DIR # the official endpoint changed to abseil/googletest
- cd $GOOGLE_TEST_DIR && git checkout release-1.8.1
- displayName: 'Install Dependencies'
- - script: |
- mkdir $BUILD_DIR && cd $BUILD_DIR
- cmake $(Build.SourcesDirectory) \
- -DCMAKE_OSX_SYSROOT="/Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX.sdk" \
- -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \
- -DGTEST_SRC_DIR="$GOOGLE_TEST_DIR/googletest" \
- -DGTEST_INCLUDE_DIR="$GOOGLE_TEST_DIR/googletest/include" \
- -DQt5_DIR=/usr/local/opt/qt5/lib/cmake/Qt5 \
- -DPCL_ONLY_CORE_POINT_TYPES=ON \
- -DBUILD_simulation=ON \
- -DBUILD_surface=OFF \
- -DBUILD_global_tests=ON \
- -DBUILD_examples=ON \
- -DBUILD_tools=ON \
- -DBUILD_apps=ON \
- -DBUILD_apps_3d_rec_framework=ON \
- -DBUILD_apps_cloud_composer=ON \
- -DBUILD_apps_in_hand_scanner=ON \
- -DBUILD_apps_modeler=ON \
- -DBUILD_apps_point_cloud_editor=ON
- displayName: 'CMake Configuration'
- - script: |
- cd $BUILD_DIR
- # Compiling some of the test targets with -j2 option leads to pipeline failures
- # (presumably out of memory error). Thus we make them separately in a single
- # thread mode. Their corresponding modules are built before with the -j2 mode
- # to make the process faster.
- cmake --build . -- -j2 pcl_filters pcl_registration
- cmake --build . -- test_filters test_registration test_registration_api
- cmake --build . -- -j2
- displayName: 'Build Library'
- - script: cd $BUILD_DIR && cmake --build . -- tests
- displayName: 'Run Unit Tests'
- - task: PublishTestResults@2
- inputs:
- testResultsFormat: 'CTest'
- testResultsFiles: '**/Test*.xml'
- searchFolder: '$(Agent.WorkFolder)/build'
- condition: succeededOrFailed()
-
+++ /dev/null
-resources:
- containers:
- - container: env1604
- image: pointcloudlibrary/env:16.04
-
-jobs:
- - job: ubuntu1604
- displayName: Ubuntu 16.04
- timeoutInMinutes: 0
- pool:
- vmImage: 'Ubuntu 16.04'
- container: env1604
- variables:
- BUILD_DIR: '$(Agent.BuildDirectory)/build'
- steps:
- - script: |
- mkdir $BUILD_DIR && cd $BUILD_DIR
- cmake $(Build.SourcesDirectory) \
- -DPCL_ONLY_CORE_POINT_TYPES=ON \
- -DPCL_WARNINGS_ARE_ERRORS=ON \
- -DBUILD_simulation=ON \
- -DBUILD_surface_on_nurbs=ON \
- -DBUILD_global_tests=ON \
- -DBUILD_examples=ON \
- -DBUILD_tools=ON \
- -DBUILD_apps=ON \
- -DBUILD_apps_3d_rec_framework=ON \
- -DBUILD_apps_cloud_composer=ON \
- -DBUILD_apps_in_hand_scanner=ON \
- -DBUILD_apps_modeler=ON \
- -DBUILD_apps_point_cloud_editor=ON
- displayName: 'CMake Configuration'
- - script: |
- cd $BUILD_DIR
- # Compiling some of the test targets with -j2 option leads to pipeline failures
- # (presumably out of memory error). Thus we make them separately in a single
- # thread mode. Their corresponding modules are built before with the -j2 mode
- # to make the process faster.
- cmake --build . -- -j2 pcl_filters pcl_registration
- cmake --build . -- test_filters test_registration test_registration_api
- cmake --build . -- -j2
- displayName: 'Build Library'
- - script: |
- cd $BUILD_DIR && cmake --build . -- tests
- displayName: 'Run Unit Tests'
- - task: PublishTestResults@2
- inputs:
- testResultsFormat: 'CTest'
- testResultsFiles: '**/Test*.xml'
- searchFolder: '$(Agent.BuildDirectory)/build'
- condition: succeededOrFailed()
+++ /dev/null
-resources:
- containers:
- - container: env1910
- image: pointcloudlibrary/env:19.10
-
-jobs:
- - job: ubuntu1910
- displayName: Ubuntu 19.10
- timeoutInMinutes: 0
- pool:
- vmImage: 'Ubuntu 16.04'
- container: env1910
- variables:
- BUILD_DIR: '$(Agent.BuildDirectory)/build'
- CMAKE_CXX_FLAGS: '-Wall -Wextra -O2'
- steps:
- - script: |
- mkdir $BUILD_DIR && cd $BUILD_DIR
- cmake $(Build.SourcesDirectory) \
- -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \
- -DPCL_ONLY_CORE_POINT_TYPES=ON \
- -DBUILD_simulation=ON \
- -DBUILD_surface_on_nurbs=ON \
- -DBUILD_global_tests=ON \
- -DBUILD_examples=ON \
- -DBUILD_tools=ON \
- -DBUILD_apps=ON \
- -DBUILD_apps_3d_rec_framework=ON \
- -DBUILD_apps_cloud_composer=ON \
- -DBUILD_apps_in_hand_scanner=ON \
- -DBUILD_apps_modeler=ON \
- -DBUILD_apps_point_cloud_editor=ON
- displayName: 'CMake Configuration'
- - script: |
- cd $BUILD_DIR
- # Compiling some of the test targets with -j2 option leads to pipeline failures
- # (presumably out of memory error). Thus we make them separately in a single
- # thread mode. Their corresponding modules are built before with the -j2 mode
- # to make the process faster.
- cmake --build . -- -j2 pcl_filters pcl_registration
- cmake --build . -- test_filters test_registration test_registration_api
- cmake --build . -- -j2
- displayName: 'Build Library'
- - script: |
- cd $BUILD_DIR && cmake --build . -- tests
- displayName: 'Run Unit Tests'
- - task: PublishTestResults@2
- inputs:
- testResultsFormat: 'CTest'
- testResultsFiles: '**/Test*.xml'
- searchFolder: '$(Agent.BuildDirectory)/build'
- condition: succeededOrFailed()
--- /dev/null
+jobs:
+ - job: ubuntu
+ displayName: Ubuntu
+ pool:
+ vmImage: 'Ubuntu 16.04'
+ strategy:
+ matrix:
+ # 16.04 Clang:
+ # CONTAINER: env1604
+ # CC: clang
+ # CXX: clang++
+ # CMAKE_ARGS: '-DPCL_WARNINGS_ARE_ERRORS=ON'
+ # 20.04 Clang:
+ # CONTAINER: env2004
+ # CC: clang
+ # CXX: clang++
+ # CMAKE_ARGS: ''
+ 16.04 GCC:
+ CONTAINER: env1604
+ CC: gcc
+ CXX: g++
+ CMAKE_ARGS: '-DPCL_WARNINGS_ARE_ERRORS=ON'
+ 20.04 GCC:
+ CONTAINER: env2004
+ CC: gcc
+ CXX: g++
+ CMAKE_ARGS: ''
+ container: $[ variables['CONTAINER'] ]
+ timeoutInMinutes: 0
+ variables:
+ BUILD_DIR: '$(Agent.BuildDirectory)/build'
+ CMAKE_CXX_FLAGS: '-Wall -Wextra'
+ steps:
+ - checkout: self
+ # find the commit hash on a quick non-forced update too
+ fetchDepth: 10
+ - script: |
+ mkdir $BUILD_DIR && cd $BUILD_DIR
+ cmake $(Build.SourcesDirectory) $(CMAKE_ARGS) \
+ -DCMAKE_BUILD_TYPE="Release" \
+ -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \
+ -DPCL_ONLY_CORE_POINT_TYPES=ON \
+ -DBUILD_simulation=ON \
+ -DBUILD_surface_on_nurbs=ON \
+ -DBUILD_global_tests=ON \
+ -DBUILD_examples=ON \
+ -DBUILD_tools=ON \
+ -DBUILD_apps=ON \
+ -DBUILD_apps_3d_rec_framework=ON \
+ -DBUILD_apps_cloud_composer=ON \
+ -DBUILD_apps_in_hand_scanner=ON \
+ -DBUILD_apps_modeler=ON \
+ -DBUILD_apps_point_cloud_editor=ON
+ # Temporary fix to ensure no tests are skipped
+ cmake $(Build.SourcesDirectory)
+ displayName: 'CMake Configuration'
+ - script: |
+ cd $BUILD_DIR
+ cmake --build . -- -j2
+ displayName: 'Build Library'
+ - script: |
+ cd $BUILD_DIR && cmake --build . -- tests
+ displayName: 'Run Unit Tests'
+ - task: PublishTestResults@2
+ inputs:
+ testResultsFormat: 'CTest'
+ testResultsFiles: '**/Test*.xml'
+ searchFolder: '$(Agent.BuildDirectory)/build'
+ condition: succeededOrFailed()
--- /dev/null
+jobs:
+ - job: vs2017
+ displayName: Windows VS2017 Build
+ pool:
+ vmImage: 'vs2017-win2016'
+ strategy:
+ matrix:
+ x86:
+ PLATFORM: 'x86'
+ ARCHITECTURE: 'x86'
+ GENERATOR: 'Visual Studio 15 2017'
+ x64:
+ PLATFORM: 'x64'
+ ARCHITECTURE: 'x86_amd64'
+ GENERATOR: 'Visual Studio 15 2017 Win64'
+ timeoutInMinutes: 0
+ variables:
+ BUILD_DIR: '$(Agent.WorkFolder)\build'
+ CONFIGURATION: 'Release'
+ VCPKG_ROOT: 'C:\vcpkg'
+ steps:
+ - checkout: self
+ # find the commit hash on a quick non-forced update too
+ fetchDepth: 10
+ - script: |
+ echo ##vso[task.setvariable variable=BOOST_ROOT]%BOOST_ROOT_1_69_0%
+ displayName: 'Set BOOST_ROOT Environment Variable'
+ - script: |
+ echo ##vso[task.prependpath]%BOOST_ROOT_1_69_0%\lib
+ displayName: 'Include Boost Libraries In System PATH'
+ - script: |
+ set
+ displayName: 'Print Environment Variables'
+ - script: |
+ vcpkg.exe install eigen3 flann gtest qhull --triplet %PLATFORM%-windows && vcpkg.exe list
+ displayName: 'Install C++ Dependencies Via Vcpkg'
+ - script: |
+ rmdir %VCPKG_ROOT%\downloads /S /Q
+ rmdir %VCPKG_ROOT%\packages /S /Q
+ displayName: 'Free Up Space'
+ - script: |
+ mkdir %BUILD_DIR% && cd %BUILD_DIR%
+ cmake $(Build.SourcesDirectory) ^
+ -G"%GENERATOR%" ^
+ -DCMAKE_BUILD_TYPE="Release" ^
+ -DCMAKE_TOOLCHAIN_FILE=%VCPKG_ROOT%\scripts\buildsystems\vcpkg.cmake ^
+ -DVCPKG_APPLOCAL_DEPS=ON ^
+ -DPCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32=ON ^
+ -DPCL_BUILD_WITH_FLANN_DYNAMIC_LINKING_WIN32=ON ^
+ -DPCL_BUILD_WITH_QHULL_DYNAMIC_LINKING_WIN32=ON ^
+ -DBUILD_global_tests=ON ^
+ -DBUILD_tools=OFF ^
+ -DBUILD_surface_on_nurbs=ON
+ displayName: 'CMake Configuration'
+ - script: cd %BUILD_DIR% && cmake --build . --config %CONFIGURATION%
+ displayName: 'Build Library'
+ - script: cd %BUILD_DIR% && cmake --build . --target tests --config %CONFIGURATION%
+ displayName: 'Run Unit Tests'
+ - task: PublishTestResults@2
+ inputs:
+ testResultsFormat: 'CTest'
+ testResultsFiles: '**/Test*.xml'
+ searchFolder: '$(Agent.WorkFolder)\build'
+ condition: succeededOrFailed()
+
+++ /dev/null
-jobs:
- - job: vs2017
- displayName: Windows VS2017 Build
- timeoutInMinutes: 0
- pool:
- vmImage: 'vs2017-win2016'
- strategy:
- matrix:
- x86:
- PLATFORM: 'x86'
- ARCHITECTURE: 'x86'
- GENERATOR: 'Visual Studio 15 2017'
- x64:
- PLATFORM: 'x64'
- ARCHITECTURE: 'x86_amd64'
- GENERATOR: 'Visual Studio 15 2017 Win64'
- variables:
- BUILD_DIR: '$(Agent.WorkFolder)\build'
- CONFIGURATION: 'Release'
- VCPKG_ROOT: 'C:\vcpkg'
- steps:
- - script: set
- displayName: 'Print Environment Variables'
- - script: |
- echo ##vso[task.prependpath]%BOOST_ROOT%\lib
- displayName: 'Update System PATH'
- - script: |
- vcpkg.exe install eigen3 flann gtest qhull --triplet %PLATFORM%-windows && vcpkg.exe list
- displayName: 'Install c++ dependencies via vcpkg'
- - script: |
- rmdir %VCPKG_ROOT%\downloads /S /Q
- rmdir %VCPKG_ROOT%\packages /S /Q
- displayName: 'Free Up Space'
- - script: |
- mkdir %BUILD_DIR% && cd %BUILD_DIR%
- cmake $(Build.SourcesDirectory) -G"%GENERATOR%" -DCMAKE_TOOLCHAIN_FILE=%VCPKG_ROOT%\scripts\buildsystems\vcpkg.cmake -DVCPKG_APPLOCAL_DEPS=ON -DPCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32=ON -DPCL_BUILD_WITH_FLANN_DYNAMIC_LINKING_WIN32=ON -DPCL_BUILD_WITH_QHULL_DYNAMIC_LINKING_WIN32=ON -DBUILD_global_tests=ON -DBUILD_tools=OFF -DBUILD_surface_on_nurbs=ON
- displayName: 'CMake Configuration'
- - script: cd %BUILD_DIR% && cmake --build . --config %CONFIGURATION%
- displayName: 'Build Library'
- - script: cd %BUILD_DIR% && cmake --build . --target tests --config %CONFIGURATION%
- displayName: 'Run Unit Tests'
- - task: PublishTestResults@2
- inputs:
- testResultsFormat: 'CTest'
- testResultsFiles: '**/Test*.xml'
- searchFolder: '$(Agent.WorkFolder)\build'
- condition: succeededOrFailed()
-
--- /dev/null
+jobs:
+ - job: documentation
+ displayName: Generate Documentation
+ pool:
+ vmImage: 'Ubuntu 16.04'
+ container: doc
+ variables:
+ BUILD_DIR: '$(Agent.BuildDirectory)/build'
+ DOC_DIR: '$(Agent.BuildDirectory)/documentation'
+ CHANGELOG: '$(Agent.TempDirectory)/changelog'
+ steps:
+ - checkout: self
+ # find the commit hash on a quick non-forced update too
+ fetchDepth: 10
+ - task: InstallSSHKey@0
+ inputs:
+ hostName: github.com
+ sshPublicKey: ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIBh5Yrau/gguTfoNALxhVX77Pgz6y6UWoJRERMKR68ee documentation@pointclouds.org
+ sshKeySecureFile: id_ed25519
+ condition: eq(variables['Build.Repository.Name'], 'PointCloudLibrary/pcl')
+ - script: |
+ $(Build.SourcesDirectory)/.dev/scripts/generate_changelog.py --with-misc > $CHANGELOG.md
+ grip --export $CHANGELOG.md $CHANGELOG.html
+ pandoc -f markdown -t plain --wrap=none $CHANGELOG.md
+ displayName: 'Generate Changelog'
+ - script: |
+ mkdir $BUILD_DIR && cd $BUILD_DIR
+ cmake $(Build.SourcesDirectory) \
+ -DDOXYGEN_USE_SHORT_NAMES=OFF \
+ -DSPHINX_HTML_FILE_SUFFIX=php \
+ -DWITH_DOCS=ON \
+ -DWITH_TUTORIALS=ON
+ displayName: 'CMake Configuration'
+ - script: |
+ cd $BUILD_DIR
+ cmake --build . -- doc tutorials advanced
+ displayName: 'Build Documentation'
+ - script: |
+ cd $BUILD_DIR
+ sed -i -r -e 's/([0-9]+)\s\.\s([0-9]+)\s/\1.\2/' doc/doxygen/html/deprecated.html
+ displayName: 'Remove extra spaces in Doxygen''s Deprecated List'
+ - script: |
+ git config --global user.email "documentation@pointclouds.org"
+ git config --global user.name "PointCloudLibrary (via Azure Pipelines)"
+ echo -e "Host github.com\n\tStrictHostKeyChecking no\n" >> ~/.ssh/config
+ git clone git@github.com:PointCloudLibrary/documentation.git $DOC_DIR
+ cd $DOC_DIR
+ cp -r $BUILD_DIR/doc/tutorials/html/* tutorials
+ cp -r $BUILD_DIR/doc/advanced/html/* advanced
+ cp -r $BUILD_DIR/doc/doxygen/html/* .
+ cp $CHANGELOG.html .
+ git add --all
+ git commit --amend --reset-author -m 'Documentation for commit $(Build.SourceVersion)' -q
+ git push --force
+ displayName: 'Push Generated Documentation To GitHub'
+ condition: and(eq(variables['Build.Repository.Name'], 'PointCloudLibrary/pcl'), eq(variables['Build.SourceBranch'], 'refs/heads/master'))
+++ /dev/null
-resources:
- containers:
- - container: doc
- image: pointcloudlibrary/doc
-
-jobs:
- - job: documentation
- displayName: Generate Documentation
- pool:
- vmImage: 'Ubuntu 16.04'
- container: doc
- variables:
- BUILD_DIR: '$(Agent.BuildDirectory)/build'
- DOC_DIR: '$(Agent.BuildDirectory)/documentation'
- steps:
- - task: InstallSSHKey@0
- inputs:
- hostName: github.com
- sshPublicKey: ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIBh5Yrau/gguTfoNALxhVX77Pgz6y6UWoJRERMKR68ee documentation@pointclouds.org
- sshKeySecureFile: id_ed25519
- - task: UsePythonVersion@0
- inputs:
- versionSpec: '3.6'
- addToPath: true
- - script: |
- mkdir $BUILD_DIR && cd $BUILD_DIR
- cmake $(Build.SourcesDirectory) \
- -DDOXYGEN_USE_SHORT_NAMES=OFF \
- -DSPHINX_HTML_FILE_SUFFIX=php \
- -DWITH_DOCS=ON \
- -DWITH_TUTORIALS=ON
- displayName: 'CMake Configuration'
- - script: |
- cd $BUILD_DIR
- cmake --build . -- doc tutorials advanced
- displayName: 'Build Documentation'
- - script: |
- git config --global user.email "documentation@pointclouds.org"
- git config --global user.name "PointCloudLibrary (via Azure Pipelines)"
- echo -e "Host github.com\n\tStrictHostKeyChecking no\n" >> ~/.ssh/config
- git clone git@github.com:PointCloudLibrary/documentation.git $DOC_DIR
- cd $DOC_DIR
- cp -r $BUILD_DIR/doc/tutorials/html/* tutorials
- cp -r $BUILD_DIR/doc/advanced/html/* advanced
- cp -r $BUILD_DIR/doc/doxygen/html/* .
- git add --all
- git commit --amend --reset-author -m 'Documentation for commit $(Build.SourceVersion)' -q
- git push --force
- displayName: 'Push Generated Documentation To GitHub'
- condition: eq(variables['Build.SourceBranch'], 'refs/heads/master')
--- /dev/null
+# Docker
+# Build a Docker image
+# https://docs.microsoft.com/azure/devops/pipelines/languages/docker
+
+trigger:
+ branches:
+ include:
+ - master
+ paths:
+ include:
+ - .dev/docker/env/Dockerfile
+ - .ci/azure-pipelines/env.yml
+
+pr:
+ paths:
+ include:
+ - .dev/docker/env/Dockerfile
+ - .ci/azure-pipelines/env.yml
+
+schedules:
+- cron: "0 0 * * 0"
+ displayName: "Sunday midnight build"
+ branches:
+ include:
+ - master
+
+resources:
+- repo: self
+
+variables:
+ dockerHub: 'PointCloudLibrary@hub.docker.com'
+ dockerHubID: "pointcloudlibrary"
+
+jobs:
+- job: BuildAndPush
+ timeoutInMinutes: 360
+ displayName: "Env"
+ pool:
+ vmImage: 'ubuntu-latest'
+ strategy:
+ matrix:
+ Ubuntu 16.04:
+ CUDA_VERSION: 9.2
+ UBUNTU_DISTRO: 16.04
+ USE_CUDA: true
+ VTK_VERSION: 6
+ tag: 16.04
+ Ubuntu 18.04:
+ CUDA_VERSION: 10.2
+ UBUNTU_DISTRO: 18.04
+ USE_CUDA: true
+ VTK_VERSION: 6
+ tag: 18.04
+ Ubuntu 20.04:
+ CUDA_VERSION: 11
+ UBUNTU_DISTRO: 20.04
+ VTK_VERSION: 7
+ # nvidia-cuda docker image has not been released for 20.04 yet
+ USE_CUDA: ""
+ tag: 20.04
+ steps:
+ - task: Docker@2
+ displayName: "Build docker image"
+ inputs:
+ command: build
+ arguments: |
+ --no-cache
+ --build-arg CUDA_VERSION=$(CUDA_VERSION)
+ --build-arg UBUNTU_DISTRO=$(UBUNTU_DISTRO)
+ --build-arg USE_CUDA=$(USE_CUDA)
+ --build-arg VTK_VERSION=$(VTK_VERSION)
+ -t $(dockerHubID)/env:$(tag)
+ dockerfile: '$(Build.SourcesDirectory)/.dev/docker/env/Dockerfile'
+ tags: "$(tag)"
+ - script: |
+ set -x
+ docker run --rm -v "$(Build.SourcesDirectory)":/pcl $(dockerHubID)/env:$(tag) bash -c ' \
+ mkdir /pcl/build && cd /pcl/build && \
+ cmake /pcl \
+ -DCMAKE_BUILD_TYPE="Release" \
+ -DPCL_ONLY_CORE_POINT_TYPES=ON \
+ -DBUILD_io:BOOL=OFF \
+ -DBUILD_kdtree:BOOL=OFF && \
+ cmake --build . -- -j2'
+ displayName: 'Verify Dockerimage'
+ - task: Docker@2
+ displayName: "Push docker image"
+ inputs:
+ command: push
+ containerRegistry: $(dockerHub)
+ repository: $(dockerHubID)/env
+ tags: "$(tag)"
+ condition: and(eq(variables['Build.Repository.Name'], 'PointCloudLibrary/pcl'),
+ eq(variables['Build.SourceBranch'], 'refs/heads/master'))
--- /dev/null
+jobs:
+ - job: formatting
+ displayName: Check code formatting
+ pool:
+ vmImage: 'Ubuntu 16.04'
+ container: fmt
+ steps:
+ - checkout: self
+ # find the commit hash on a quick non-forced update too
+ fetchDepth: 10
+ - script: ./.dev/format.sh $(which clang-format) .
+ displayName: 'Run clang-format'
+ - script: git diff > formatting.patch
+ displayName: 'Compute diff'
+ - script: cat formatting.patch
+ displayName: 'Show diff'
+ - task: CopyFiles@2
+ inputs:
+ sourceFolder: '$(Build.SourcesDirectory)'
+ contents: 'formatting.patch'
+ targetFolder: '$(Build.ArtifactStagingDirectory)'
+ displayName: 'Copy diff to staging directory'
+ - task: PublishBuildArtifacts@1
+ inputs:
+ pathtoPublish: '$(Build.ArtifactStagingDirectory)'
+ artifactName: formatting
+ displayName: 'Publish diff'
+ - script: "[ ! -s formatting.patch ]"
+ displayName: 'Set job exit status'
+++ /dev/null
-resources:
- containers:
- - container: fmt
- image: pointcloudlibrary/fmt
-
-jobs:
- - job: formatting
- displayName: Check code formatting
- pool:
- vmImage: 'Ubuntu 16.04'
- container: fmt
- steps:
- - script: ./.dev/format.sh $(which clang-format-8) .
- displayName: 'Run clang-format'
- - script: git diff > formatting.patch
- displayName: 'Compute diff'
- - script: cat formatting.patch
- displayName: 'Show diff'
- - task: CopyFiles@2
- inputs:
- sourceFolder: '$(Build.SourcesDirectory)'
- contents: 'formatting.patch'
- targetFolder: '$(Build.ArtifactStagingDirectory)'
- displayName: 'Copy diff to staging directory'
- - task: PublishBuildArtifacts@1
- inputs:
- pathtoPublish: '$(Build.ArtifactStagingDirectory)'
- artifactName: formatting
- displayName: 'Publish diff'
- - script: "[ ! -s formatting.patch ]"
- displayName: 'Set job exit status'
--- /dev/null
+jobs:
+ - job: tutorials
+ displayName: Building Tutorials
+ pool:
+ vmImage: 'Ubuntu 16.04'
+ container: env1604
+ timeoutInMinutes: 0
+ variables:
+ BUILD_DIR: '$(Agent.BuildDirectory)/build'
+ INSTALL_DIR: '$(Agent.BuildDirectory)/install'
+ CMAKE_CXX_FLAGS: '-Wall -Wextra -Wabi'
+ EXCLUDE_TUTORIALS: 'davidsdk,ensenso_cameras,gpu'
+ steps:
+ - checkout: self
+ # find the commit hash on a quick non-forced update too
+ fetchDepth: 10
+ - script: |
+ mkdir $BUILD_DIR && cd $BUILD_DIR
+ cmake $(Build.SourcesDirectory) \
+ -DCMAKE_BUILD_TYPE="Release" \
+ -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \
+ -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \
+ -DPCL_ONLY_CORE_POINT_TYPES=ON \
+ -DPCL_NO_PRECOMPILE=ON \
+ -DBUILD_surface_on_nurbs=ON \
+ -DBUILD_global_tests=OFF \
+ -DBUILD_tools=OFF \
+ -DBUILD_examples=OFF \
+ -DBUILD_outofcore=OFF \
+ -DBUILD_stereo=OFF \
+ -DBUILD_simulation=OFF
+ displayName: 'CMake Configuration'
+ - script: |
+ cd $BUILD_DIR
+ cmake --build . -- -j2
+ displayName: 'Build Library'
+ - script: |
+ cd $BUILD_DIR
+ cmake --build . -- install
+ displayName: 'Install PCL'
+ - script: |
+ cd $BUILD_DIR
+ major=$(find . -type f -exec sed -n -e \
+ 's,#define PCL_MAJOR_VERSION \([0-9]\+\),\1,p' {} \;)
+ minor=$(find . -type f -exec sed -n -e \
+ 's,#define PCL_MINOR_VERSION \([0-9]\+\),\1,p' {} \;)
+ $(Build.SourcesDirectory)/.ci/scripts/build_tutorials.sh \
+ -k -s \
+ -e $EXCLUDE_TUTORIALS \
+ $INSTALL_DIR/share/pcl-${major}.${minor} \
+ $(Build.SourcesDirectory) \
+ $BUILD_DIR/tutorials
+ displayName: 'Build Tutorials'
+++ /dev/null
-resources:
- containers:
- - container: env1604
- image: pointcloudlibrary/env:16.04
-
-jobs:
- - job: tutorials
- displayName: Tutorials
- timeoutInMinutes: 0
- pool:
- vmImage: 'Ubuntu 16.04'
- container: env1604
- variables:
- BUILD_DIR: '$(Agent.BuildDirectory)/build'
- INSTALL_DIR: '$(Agent.BuildDirectory)/install'
- CMAKE_CXX_FLAGS: '-Wall -Wextra -Wabi -O2'
- EXCLUDE_TUTORIALS: 'davidsdk,ensenso_cameras,gpu'
- steps:
- - script: |
- mkdir $BUILD_DIR && cd $BUILD_DIR
- cmake $(Build.SourcesDirectory) \
- -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \
- -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \
- -DPCL_ONLY_CORE_POINT_TYPES=ON \
- -DPCL_NO_PRECOMPILE=ON \
- -DBUILD_surface_on_nurbs=ON \
- -DBUILD_global_tests=OFF \
- -DBUILD_tools=OFF \
- -DBUILD_examples=OFF \
- -DBUILD_outofcore=OFF \
- -DBUILD_stereo=OFF \
- -DBUILD_simulation=OFF
- displayName: 'CMake Configuration'
- - script: |
- cd $BUILD_DIR
- cmake --build . -- -j2
- displayName: 'Build Library'
- - script: |
- cd $BUILD_DIR
- cmake --build . -- install
- displayName: 'Install PCL'
- - script: |
- $(Build.SourcesDirectory)/.ci/scripts/build_tutorials.sh -k -s -e $EXCLUDE_TUTORIALS $INSTALL_DIR/share/pcl-1.10 $(Build.SourcesDirectory) $BUILD_DIR/tutorials
- displayName: 'Build Tutorials'
---
AlwaysBreakAfterReturnType: All
-AlwaysBreakTemplateDeclarations: true
+AlwaysBreakTemplateDeclarations: Yes
BinPackArguments: false
BinPackParameters: false
BraceWrapping:
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 0
Language: Cpp
+NamespaceIndentation: None
PointerAlignment: Left
-Standard: Cpp11
+Standard: c++14
TabWidth: 2
UseTab: Never
+IncludeBlocks: Regroup
+IncludeCategories:
+# Main PCL includes of common module should be sorted at end of PCL includes
+ - Regex: '^<pcl/[^/]+>$'
+ Priority: 100
+ SortPriority: 101
+# All other PCL includes, which are grouped into modules
+ - Regex: '^<pcl/.*/.*>$'
+ Priority: 100
+ SortPriority: 100
+# Major 3rd-Party components of tests & modules
+ - Regex: '^<gtest/'
+ Priority: 200
+ - Regex: '^<boost/'
+ Priority: 210
+ - Regex: '^<(unsupported/)?Eigen/'
+ Priority: 220
+ - Regex: '^<flann/'
+ Priority: 230
+# Major 3rd-Party components of apps
+ - Regex: '^<Q[^/]+>$'
+ Priority: 300
+ - Regex: '^<ui_[^/]+\.h>$'
+ Priority: 300
+ - Regex: '^<vtk[^/]+\.h>$'
+ Priority: 310
+# Minor 3rd-Party components
+ - Regex: '^<librealsense2/'
+ Priority: 400
+ - Regex: '^<(ros|message_filters)/'
+ Priority: 410
+ - Regex: '^<opencv(2)?/'
+ Priority: 420
+ - Regex: '^<tide/'
+ Priority: 430
+ - Regex: '^<thrust/'
+ Priority: 440
+ - Regex: '^<(OpenGL|(GL(UT)?/))'
+ Priority: 450
+# Matches all std includes. Match them before any unknown include, so we can order them behind.
+ - Regex: '^<[a-z]+>$'
+ Priority: 900
+# Any unknown include
+ - Regex: '.*'
+ Priority: 500
-FROM pointcloudlibrary/env:19.04
+FROM pointcloudlibrary/env:20.04
ENV DEBIAN_FRONTEND=noninteractive
dvipng \
git \
python3-pip \
+ pandoc \
&& rm -rf /var/lib/apt/lists/*
-RUN pip3 install Jinja2==2.8.1 sphinx sphinxcontrib-doxylink
+RUN pip3 install Jinja2 sphinx sphinxcontrib-doxylink sphinx_rtd_theme requests grip
&& apt-get install -y \
cmake \
g++ \
+ clang \
wget \
libboost-date-time-dev \
libboost-filesystem-dev \
-FROM ubuntu:19.04
+# Azure needs node shadow, sudo and the label
+FROM node:lts-alpine
-ENV DEBIAN_FRONTEND=noninteractive
-ARG CLANG_FORMAT_VERSION=8
+# clang-10 needed alpine edge as of 2020-Apr-28
+RUN apk add \
+ --no-cache \
+ --repository=http://dl-cdn.alpinelinux.org/alpine/edge/main \
+ bash clang git shadow sudo
-RUN apt-get update \
- && apt-get install -y \
- clang-format-${CLANG_FORMAT_VERSION} \
- bash \
- git \
- && rm -rf /var/lib/apt/lists/*
+LABEL "com.azure.dev.pipelines.agent.handler.node.path"="/usr/local/bin/node"
+
+CMD [ "bash" ]
format() {
# don't use a directory with whitespace
- local whitelist="2d ml octree simulation stereo"
+ local whitelist="apps/modeler 2d ml octree simulation stereo"
local PCL_DIR="${2}"
local formatter="${1}"
--- /dev/null
+#! /usr/bin/env python3
+
+"""
+Software License Agreement (BSD License)
+
+ Point Cloud Library (PCL) - www.pointclouds.org
+ Copyright (c) 2018-, Open Perception, Inc.
+
+ All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions
+ are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above
+ copyright notice, this list of conditions and the following
+ disclaimer in the documentation and/or other materials provided
+ with the distribution.
+ * Neither the name of the copyright holder(s) nor the names of its
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGE.
+
+"""
+
+import json
+import argparse
+from pathlib import Path
+
+import requests
+import re
+
+
+CATEGORIES = {
+ "new feature": ("New features", ""),
+ "deprecation": (
+ "Deprecation",
+ "of public APIs, scheduled to be removed after two minor releases",
+ ),
+ "removal": ("Removal", "of the public APIs deprecated in previous releases"),
+ "behavior change": ("Behavior changes", "in classes, apps, or tools",),
+ "API break": (
+ "API changes",
+ "that did not go through the proper deprecation and removal cycle",
+ ),
+ "ABI break": ("ABI changes", "that are still API compatible",),
+ "fix": (None, None),
+ "enhancement": (None, None),
+}
+
+
+MODULES = {
+ "cmake": "CMake",
+ "2d": "libpcl_2d",
+ "common": "libpcl_common",
+ "cuda": "libpcl_cuda",
+ "features": "libpcl_features",
+ "filters": "libpcl_filters",
+ "geometry": "libpcl_geometry",
+ "gpu": "libpcl_gpu",
+ "io": "libpcl_io",
+ "kdtree": "libpcl_kdtree",
+ "keypoints": "libpcl_keypoints",
+ "ml": "libpcl_ml",
+ "octree": "libpcl_octree",
+ "outofcore": "libpcl_outofcore",
+ "people": "libpcl_people",
+ "recognition": "libpcl_recognition",
+ "registration": "libpcl_registration",
+ "sample_consensus": "libpcl_sample_consensus",
+ "search": "libpcl_search",
+ "segmentation": "libpcl_segmentation",
+ "simulation": "libpcl_simulation",
+ "stereo": "libpcl_stereo",
+ "surface": "libpcl_surface",
+ "visualization": "libpcl_visualization",
+ "apps": "PCL Apps",
+ "docs": "PCL Docs",
+ "tutorials": "PCL Tutorials",
+ "examples": "PCL Examples",
+ "tests": "PCL Tests",
+ "tools": "PCL Tools",
+ "ci": "CI",
+ None: "Uncategorized",
+}
+
+
+def fetch_latest_release_date():
+ """
+ Fetch the date when the latest release was created.
+ """
+ url = "https://api.github.com/repos/PointCloudLibrary/pcl/releases/latest"
+ response = requests.get(url)
+ response.raise_for_status()
+ return response.json()["created_at"]
+
+
+def fetch_pr_since(start_date):
+ """
+ Fetch data of PRs merged after a given start date.
+ """
+ url = f"https://api.github.com/search/issues?q=is:pr+repo:PointCloudLibrary/pcl+merged:>={start_date}"
+ pr_data = list()
+ page = 1
+ while True:
+ response = requests.get(f"{url}&page={page}&per_page=100")
+ response.raise_for_status()
+ data = response.json()
+ total_count = data["total_count"]
+ pr_data.extend(data["items"])
+ page += 1
+ if len(pr_data) == total_count:
+ break
+ return pr_data
+
+
+def filter_labels(labels, prefix):
+ """
+ Filter a given list of PR labels, keeping only those starting with a given prefix.
+ The prefix is stripped from the labels.
+ """
+ return [
+ label["name"][len(prefix) :]
+ for label in labels
+ if label["name"].startswith(prefix)
+ ]
+
+
+def strip_leading_tag(text):
+ """
+ >>> strip_leading_tag("[text] larger text")
+ 'larger text'
+ >>> strip_leading_tag("no tag text")
+ 'no tag text'
+ """
+ if len(text) == 0 or text[0] != '[':
+ return text
+ pattern = re.compile('\[.*\]\s*')
+ match = pattern.match(text)
+ return text[match.end():] if match else text
+
+
+def make_pr_bullet_point(pr, prefix=None):
+ ref = "[#{0}](https://github.com/PointCloudLibrary/pcl/pull/{0})".format(
+ pr["number"]
+ )
+
+ tags = ""
+ if prefix in ("modules", "categories"):
+ tags = "".join(["[" + k + "]" for k in pr[prefix]])
+ if tags:
+ tags = "**" + tags + "** "
+
+ return f"* {tags}{pr['title']} [{ref}]"
+
+
+def generate_category_section(key, prs):
+ section = list()
+ filtered_prs = [pr for pr in prs if key in pr["categories"]]
+ title, description = CATEGORIES[key]
+ if filtered_prs and title:
+ section += [f"\n**{title}** *{description}*\n"]
+ section += [make_pr_bullet_point(pr, "modules") for pr in filtered_prs]
+ return section
+
+
+def generate_module_section(key, prs):
+ section = list()
+ filtered_prs = [pr for pr in prs if key in pr["modules"]]
+ title = MODULES[key]
+ if filtered_prs and title:
+ section += [f"\n#### {title}:\n"]
+ section += [make_pr_bullet_point(pr, "categories") for pr in filtered_prs]
+ return section
+
+
+if __name__ == "__main__":
+ parser = argparse.ArgumentParser(
+ description="""
+ Generate changelog from pull requests merged after the latest release.
+ """,
+ formatter_class=argparse.RawDescriptionHelpFormatter,
+ )
+ cache_grp = parser.add_mutually_exclusive_group()
+ cache_grp.add_argument("--save", type=Path, help="Save PR data fetched from GitHub")
+ cache_grp.add_argument("--load", type=Path, help="Load PR data from a file")
+ parser.add_argument(
+ "--with-misc",
+ "-m",
+ action="store_true",
+ help=(
+ "Add section with miscellaneous PRs that are not important enough to be "
+ "included in the official changelog"
+ ),
+ )
+ args = parser.parse_args()
+
+ if args.load:
+ with open(args.load) as fp:
+ pr_data = json.loads(fp.read())
+ else:
+ date = fetch_latest_release_date()
+ pr_data = fetch_pr_since(date)
+ if args.save:
+ with open(args.save, "w") as fp:
+ fp.write(json.dumps(pr_data))
+
+ selected_prs = list()
+ excluded_prs = list()
+ for pr in sorted(pr_data, key=lambda d: d["closed_at"]):
+ categories = filter_labels(pr["labels"], "changelog: ")
+ title = strip_leading_tag(pr["title"])
+ pr_info = {
+ "number": pr["number"],
+ "title": title,
+ "modules": filter_labels(pr["labels"], "module: "),
+ "categories": [g for g in categories if g not in ["fix", "enhancement"]],
+ }
+ if categories:
+ selected_prs.append(pr_info)
+ else: # exclude PRs not tagged with any changelog label
+ excluded_prs.append(pr_info)
+
+ clog = list()
+
+ clog += ["\n### Notable changes"]
+ for k in CATEGORIES.keys():
+ clog.extend(generate_category_section(k, selected_prs))
+
+ clog += ["\n### Changes grouped by module"]
+ for k in MODULES.keys():
+ clog.extend(generate_module_section(k, selected_prs))
+
+ if args.with_misc:
+ clog += ["\n### Miscellaneous PRs excluded from changelog\n"]
+ for pr in excluded_prs:
+ clog += [make_pr_bullet_point(pr)]
+
+ print("\n".join(clog))
+++ /dev/null
-#! /usr/bin/env python3
-
-"""
-Software License Agreement (BSD License)
-
- Point Cloud Library (PCL) - www.pointclouds.org
- Copyright (c) 2018-, Open Perception, Inc.
-
- All rights reserved.
-
- Redistribution and use in source and binary forms, with or without
- modification, are permitted provided that the following conditions
- are met:
-
- * Redistributions of source code must retain the above copyright
- notice, this list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above
- copyright notice, this list of conditions and the following
- disclaimer in the documentation and/or other materials provided
- with the distribution.
- * Neither the name of the copyright holder(s) nor the names of its
- contributors may be used to endorse or promote products derived
- from this software without specific prior written permission.
-
- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- POSSIBILITY OF SUCH DAMAGE.
-
-"""
-
-import argparse
-from argparse import ArgumentParser
-from collections import defaultdict
-import getpass
-import json
-import os
-import pathlib
-import re
-import subprocess
-import sys
-
-import requests
-
-
-def find_pcl_folder():
- folder = os.path.dirname(os.path.abspath(__file__))
- folder = pathlib.Path(folder).parent
- return str(folder)
-
-
-def find_pr_list(start: str, end: str):
- """Returns all PR ids from a certain commit range. Inspired in
- http://joey.aghion.com/find-the-github-pull-request-for-a-commit/
- https://stackoverflow.com/questions/36433572/how-does-ancestry-path-work-with-git-log#36437843
- """
-
- # Let git generate the proper pr history
- cmd = "git log --oneline " + start + ".." + end
- cmd = cmd.split()
- output = subprocess.run(cmd, cwd=FOLDER, stdout=subprocess.PIPE)
- pr_commits = output.stdout.split(b"\n")
-
- # Fetch ids for all merge requests from PRS
- merge_re = re.compile("\S+ Merge pull request #(\d+) from \S+")
- squash_re = re.compile("\(#(\d+)\)")
-
- ids = []
- for pr in pr_commits:
-
- pr_s = str(pr)
-
- # Match agains usual pattern
- uid = None
- match = merge_re.fullmatch(pr_s)
-
- # Match agains squash pattern
- if not match:
- match = squash_re.search(pr_s)
-
- # Abort
- if not match:
- continue
-
- # Extract PR uid
- uid = int(match.group(1))
- ids.append(uid)
-
- return ids
-
-
-def fetch_pr_info(pr_ids, auth):
-
- prs_url = "https://api.github.com/repos/PointCloudLibrary/pcl/pulls/"
- pr_info = []
-
- sys.stdout.write("Fetching PR Info: {}%".format(0))
- sys.stdout.flush()
-
- for i, pr_id in enumerate(pr_ids):
-
- # Fetch GitHub info
- response = requests.get(prs_url + str(pr_id), auth=auth)
- data = response.json()
-
- if response.status_code != 200:
- print(
- "\nError: Failed to fetch PR info. Server reported '"
- + data["message"]
- + "'",
- file=sys.stderr,
- )
- exit(code=1)
-
- d = {"id": pr_id, "title": data["title"], "labels": data["labels"]}
- pr_info.append(d)
-
- # import pdb; pdb.set_trace()
- sys.stdout.write(
- "\rFetching PR Info: {:0.2f}%".format(100 * (i + 1) / len(pr_ids))
- )
- sys.stdout.flush()
-
- print("")
- return pr_info
-
-
-def extract_version(tag):
- """Finds the corresponding version from a provided tag.
- If the tag does not correspond to a suitable version tag, the original tag
- is returned
- """
- version_re = re.compile("pcl-\S+")
- res = version_re.fullmatch(tag)
-
- # Not a usual version tag
- if not res:
- return tag
-
- return tag[4:]
-
-
-def generate_text_content(tag, pr_info):
-
- module_order = (
- None,
- "cmake",
- "2d",
- "common",
- "cuda",
- "features",
- "filters",
- "geometry",
- "gpu",
- "io",
- "kdtree",
- "keypoints",
- "ml",
- "octree",
- "outofcore",
- "people",
- "recognition",
- "registration",
- "sample_consensus",
- "search",
- "segmentation",
- "simulation",
- "stereo",
- "surface",
- "visualization",
- "apps",
- "docs",
- "tutorials",
- "examples",
- "tests",
- "tools",
- "ci",
- )
-
- module_titles = {
- None: "Uncategorized",
- "2d": "libpcl_2d",
- "apps": "PCL Apps",
- "cmake": "CMake",
- "ci": "CI",
- "common": "libpcl_common",
- "cuda": "libpcl_cuda",
- "docs": "PCL Docs",
- "examples": "PCL Examples",
- "features": "libpcl_features",
- "filters": "libpcl_filters",
- "gpu": "libpcl_gpu",
- "io": "libpcl_io",
- "kdtree": "libpcl_kdtree",
- "keypoints": "libpcl_keypoints",
- "ml": "libpcl_ml",
- "octree": "libpcl_octree",
- "outofcore": "libpcl_outofcore",
- "people": "libpcl_people",
- "recognition": "libpcl_recognition",
- "registration": "libpcl_registration",
- "sample_consensus": "libpcl_sample_consensus",
- "search": "libpcl_search",
- "segmentation": "libpcl_segmentation",
- "simulation": "libpcl_simulation",
- "stereo": "libpcl_stereo",
- "surface": "libpcl_surface",
- "tests": "PCL Tests",
- "tools": "PCL Tools",
- "tutorials": "PCL Tutorials",
- "visualization": "libpcl_visualization",
- }
-
- changes_order = ("new-feature", "deprecation", "removal", "behavior", "api", "abi")
-
- changes_titles = {
- "new-feature": "New Features",
- "deprecation": "Deprecated",
- "removal": "Removed",
- "behavior": "Behavioral changes",
- "api": "API changes",
- "abi": "ABI changes",
- }
-
- changes_description = {
- "new-feature": "Newly added functionalities.",
- "deprecation": "Deprecated code scheduled to be removed after two minor releases.",
- "removal": "Removal of deprecated code.",
- "behavior": "Changes in the expected default behavior.",
- "api": "Changes to the API which didn't went through the proper deprecation and removal cycle.",
- "abi": "Changes that cause ABI incompatibility but are still API compatible.",
- }
-
- changes_labels = {
- "breaks API": "api",
- "breaks ABI": "abi",
- "behavior": "behavior",
- "deprecation": "deprecation",
- "removal": "removal",
- }
-
- # change_log content
- clog = []
-
- # Infer version from tag
- version = extract_version(tag)
-
- # Find the commit date for writting the Title
- cmd = ("git log -1 --format=%ai " + tag).split()
- output = subprocess.run(cmd, cwd=FOLDER, stdout=subprocess.PIPE)
- date = output.stdout.split()[0].decode()
- tokens = date.split("-")
- clog += [
- "## *= "
- + version
- + " ("
- + tokens[2]
- + "."
- + tokens[1]
- + "."
- + tokens[0]
- + ") =*"
- ]
-
- # Map each PR into the approriate module and changes
- modules = defaultdict(list)
- changes = defaultdict(list)
- module_re = re.compile("module: \S+")
- changes_re = re.compile("changes: ")
- feature_re = re.compile("new feature")
-
- for pr in pr_info:
-
- pr["modules"] = []
- pr["changes"] = []
-
- for label in pr["labels"]:
- if module_re.fullmatch(label["name"]):
- module = label["name"][8:]
- pr["modules"].append(module)
- modules[module].append(pr)
-
- elif changes_re.match(label["name"]):
- key = changes_labels[label["name"][9:]]
- pr["changes"].append(key)
- changes[key].append(pr)
-
- elif feature_re.fullmatch(label["name"]):
- pr["changes"].append("new-feature")
- changes["new-feature"].append(pr)
-
- # No labels defaults to section None
- if not pr["modules"]:
- modules[None].append(pr)
- continue
-
- # Generate Changes Summary
- for key in changes_order:
-
- # Skip empty sections
- if not changes[key]:
- continue
-
- clog += ["\n### `" + changes_titles[key] + ":`\n"]
-
- clog += ["*" + changes_description[key] + "*\n"]
-
- for pr in changes[key]:
- prefix = "".join(["[" + k + "]" for k in pr["modules"]])
- if prefix:
- prefix = "**" + prefix + "** "
- clog += [
- "* "
- + prefix
- + pr["title"]
- + " [[#"
- + str(pr["id"])
- + "]]"
- + "(https://github.com/PointCloudLibrary/pcl/pull/"
- + str(pr["id"])
- + ")"
- ]
-
- # Traverse Modules and generate each section's content
- clog += ["\n### `Modules:`"]
- for key in module_order:
-
- # Skip empty sections
- if not modules[key]:
- continue
-
- # if key:
- clog += ["\n#### `" + module_titles[key] + ":`\n"]
-
- for pr in modules[key]:
- prefix = "".join(["[" + k + "]" for k in pr["changes"]])
- if prefix:
- prefix = "**" + prefix + "** "
- clog += [
- "* "
- + prefix
- + pr["title"]
- + " [[#"
- + str(pr["id"])
- + "]]"
- + "(https://github.com/PointCloudLibrary/pcl/pull/"
- + str(pr["id"])
- + ")"
- ]
-
- return clog
-
-
-def parse_arguments():
-
- parser = ArgumentParser(
- description="Generate a change log between two "
- "revisions.\n\nCheck https://github.com/PointCloudLibrary/pcl/wiki/Preparing-Releases#creating-the-change-log "
- "for some additional examples on how to use the tool."
- )
- parser.add_argument(
- "start",
- help="The start (exclusive) " "revision/commit/tag to generate the log.",
- )
- parser.add_argument(
- "end",
- nargs="?",
- default="HEAD",
- help="The end "
- "(inclusive) revision/commit/tag to generate the log. "
- "(Defaults to HEAD)",
- )
- parser.add_argument(
- "--username",
- help="GitHub Account user name. If "
- "specified it will perform requests with the provided credentials. "
- "This is often required in order to overcome GitHub API's request "
- "limits.",
- )
- meg = parser.add_mutually_exclusive_group()
- meg.add_argument(
- "--cache",
- nargs="?",
- const="pr_info.json",
- metavar="FILE",
- help="Caches the PR info extracted from GitHub into a JSON file. "
- "(Defaults to 'pr_info.json')",
- )
- meg.add_argument(
- "--from-cache",
- nargs="?",
- const="pr_info.json",
- metavar="FILE",
- help="Uses a previously generated PR info JSON cache "
- "file to generate the change log. (Defaults to 'pr_info.json')",
- )
-
- # Parse arguments
- args = parser.parse_args()
- args.auth = None
-
- if args.username:
- password = getpass.getpass(prompt="Password for " + args.username + ": ")
- args.auth = (args.username, password)
-
- return args
-
-
-##
-## 'main'
-##
-
-FOLDER = find_pcl_folder()
-
-# Parse arguments
-args = parse_arguments()
-
-pr_info = None
-if not args.from_cache:
-
- # Find all PRs since tag
- prs = find_pr_list(start=args.start, end=args.end)
-
- # Generate pr objects with title, labels from ids
- pr_info = fetch_pr_info(prs, auth=args.auth)
- if args.cache:
- with open(args.cache, "w") as fp:
- d = {"start": args.start, "end": args.end, "pr_info": pr_info}
- fp.write(json.dumps(d))
-else:
- # Load previously cached info
- with open(args.from_cache) as fp:
- d = json.loads(fp.read())
- pr_info = d["pr_info"]
- args.start = d["start"]
- args.end = d["start"]
-
-
-# Generate text content based on changes
-clog = generate_text_content(tag=args.end, pr_info=pr_info)
-print("\n".join(clog))
#pragma once
#include <pcl/filters/filter.h>
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/2d/convolution.h>
#include <pcl/2d/kernel.h>
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
*
*/
-#ifndef PCL_2D_CONVOLUTION_IMPL_HPP
-#define PCL_2D_CONVOLUTION_IMPL_HPP
+#pragma once
+
+#include <pcl/2d/convolution.h>
+
+namespace pcl {
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Convolution<PointT>::filter(pcl::PointCloud<PointT>& output)
+Convolution<PointT>::filter(pcl::PointCloud<PointT>& output)
{
int input_row = 0;
int input_col = 0;
}
} // switch
}
-
-#endif
+} // namespace pcl
*
*/
-#ifndef PCL_2D_EDGE_IMPL_HPP
-#define PCL_2D_EDGE_IMPL_HPP
+#pragma once
#include <pcl/2d/convolution.h>
+#include <pcl/2d/edge.h>
#include <pcl/common/common_headers.h> // rad2deg()
-//////////////////////////////////////////////////////////////////////////////
+namespace pcl {
+
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::detectEdgeSobel(pcl::PointCloud<PointOutT>& output)
+Edge<PointInT, PointOutT>::detectEdgeSobel(pcl::PointCloud<PointOutT>& output)
{
convolution_.setInputCloud(input_);
pcl::PointCloud<PointXYZI>::Ptr kernel_x(new pcl::PointCloud<PointXYZI>);
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::sobelMagnitudeDirection(
+Edge<PointInT, PointOutT>::sobelMagnitudeDirection(
const pcl::PointCloud<PointInT>& input_x,
const pcl::PointCloud<PointInT>& input_y,
pcl::PointCloud<PointOutT>& output)
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::detectEdgePrewitt(pcl::PointCloud<PointOutT>& output)
+Edge<PointInT, PointOutT>::detectEdgePrewitt(pcl::PointCloud<PointOutT>& output)
{
convolution_.setInputCloud(input_);
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::detectEdgeRoberts(pcl::PointCloud<PointOutT>& output)
+Edge<PointInT, PointOutT>::detectEdgeRoberts(pcl::PointCloud<PointOutT>& output)
{
convolution_.setInputCloud(input_);
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::cannyTraceEdge(
+Edge<PointInT, PointOutT>::cannyTraceEdge(
int rowOffset, int colOffset, int row, int col, pcl::PointCloud<PointXYZI>& maxima)
{
int newRow = row + rowOffset;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::discretizeAngles(pcl::PointCloud<PointOutT>& thet)
+Edge<PointInT, PointOutT>::discretizeAngles(pcl::PointCloud<PointOutT>& thet)
{
const int height = thet.height;
const int width = thet.width;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::suppressNonMaxima(
+Edge<PointInT, PointOutT>::suppressNonMaxima(
const pcl::PointCloud<PointXYZIEdge>& edges,
pcl::PointCloud<PointXYZI>& maxima,
float tLow)
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::detectEdgeCanny(pcl::PointCloud<PointOutT>& output)
+Edge<PointInT, PointOutT>::detectEdgeCanny(pcl::PointCloud<PointOutT>& output)
{
float tHigh = hysteresis_threshold_high_;
float tLow = hysteresis_threshold_low_;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::canny(const pcl::PointCloud<PointInT>& input_x,
-
const pcl::PointCloud<PointInT>& input_y,
-
pcl::PointCloud<PointOutT>& output)
+Edge<PointInT, PointOutT>::canny(const pcl::PointCloud<PointInT>& input_x,
+ const pcl::PointCloud<PointInT>& input_y,
+ pcl::PointCloud<PointOutT>& output)
{
float tHigh = hysteresis_threshold_high_;
float tLow = hysteresis_threshold_low_;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
void
-
pcl::Edge<PointInT, PointOutT>::detectEdgeLoG(const float kernel_sigma,
- const float kernel_size,
-
pcl::PointCloud<PointOutT>& output)
+Edge<PointInT, PointOutT>::detectEdgeLoG(const float kernel_sigma,
+ const float kernel_size,
+ pcl::PointCloud<PointOutT>& output)
{
convolution_.setInputCloud(input_);
convolution_.filter(output);
}
-#endif
+} // namespace pcl
*
*/
-#ifndef PCL_2D_KERNEL_IMPL_HPP
-#define PCL_2D_KERNEL_IMPL_HPP
+#pragma once
+
+#include <pcl/2d/kernel.h>
+
+namespace pcl {
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::fetchKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::fetchKernel(pcl::PointCloud<PointT>& kernel)
{
switch (kernel_type_) {
case SOBEL_X:
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::gaussianKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::gaussianKernel(pcl::PointCloud<PointT>& kernel)
{
float sum = 0;
kernel.resize(kernel_size_ * kernel_size_);
kernel[i].intensity /= sum;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::loGKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::loGKernel(pcl::PointCloud<PointT>& kernel)
{
float sum = 0;
- float temp = 0;
kernel.resize(kernel_size_ * kernel_size_);
kernel.height = kernel_size_;
kernel.width = kernel_size_;
for (int j = 0; j < kernel_size_; j++) {
int iks = (i - kernel_size_ / 2);
int jks = (j - kernel_size_ / 2);
- temp = float(double(iks * iks + jks * jks) / sigma_sqr);
+ float temp = float(double(iks * iks + jks * jks) / sigma_sqr);
kernel(j, i).intensity = (1.0f - temp) * std::exp(-temp);
sum += kernel(j, i).intensity;
}
kernel[i].intensity /= sum;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::sobelKernelX(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::sobelKernelX(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(9);
kernel.height = 3;
kernel(2, 2).intensity = 1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::prewittKernelX(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::prewittKernelX(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(9);
kernel.height = 3;
kernel(2, 2).intensity = 1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::robertsKernelX(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::robertsKernelX(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(4);
kernel.height = 2;
kernel(1, 1).intensity = -1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::sobelKernelY(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::sobelKernelY(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(9);
kernel.height = 3;
kernel(2, 2).intensity = 1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::prewittKernelY(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::prewittKernelY(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(9);
kernel.height = 3;
template <typename PointT>
void
-
pcl::kernel<PointT>::robertsKernelY(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::robertsKernelY(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(4);
kernel.height = 2;
kernel(1, 1).intensity = 0;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::derivativeXCentralKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::derivativeXCentralKernel(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(3);
kernel.height = 1;
kernel(2, 0).intensity = 1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::derivativeXForwardKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::derivativeXForwardKernel(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(3);
kernel.height = 1;
kernel(2, 0).intensity = 1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::derivativeXBackwardKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::derivativeXBackwardKernel(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(3);
kernel.height = 1;
kernel(2, 0).intensity = 0;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::derivativeYCentralKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::derivativeYCentralKernel(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(3);
kernel.height = 3;
kernel(0, 2).intensity = 1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::derivativeYForwardKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::derivativeYForwardKernel(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(3);
kernel.height = 3;
kernel(0, 2).intensity = 1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::derivativeYBackwardKernel(pcl::PointCloud<PointT>& kernel)
+kernel<PointT>::derivativeYBackwardKernel(pcl::PointCloud<PointT>& kernel)
{
kernel.resize(3);
kernel.height = 3;
kernel(0, 2).intensity = 0;
}
-//////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::setKernelType(KERNEL_ENUM kernel_type)
+kernel<PointT>::setKernelType(KERNEL_ENUM kernel_type)
{
kernel_type_ = kernel_type;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::setKernelSize(int kernel_size)
+kernel<PointT>::setKernelSize(int kernel_size)
{
kernel_size_ = kernel_size;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::kernel<PointT>::setKernelSigma(float kernel_sigma)
+kernel<PointT>::setKernelSigma(float kernel_sigma)
{
sigma_ = kernel_sigma;
}
-#endif
+} // namespace pcl
* Author: somani
*/
-#ifndef PCL_2D_KEYPOINT_HPP_
-#define PCL_2D_KEYPOINT_HPP_
+#pragma once
-#include <limits>
#include <pcl/2d/convolution.h>
#include <pcl/2d/edge.h>
+#include <pcl/2d/keypoint.h> // for pcl::Keypoint
+
+#include <limits>
-//////////////////////////////////////////////////////////////////////////////
+namespace pcl {
+
+template <typename ImageType>
void
-pcl::keypoint::harrisCorner(ImageType& output,
- ImageType& input,
- const float sigma_d,
- const float sigma_i,
- const float alpha,
- const float thresh)
+Keypoint<ImageType>::harrisCorner(ImageType& output,
+ ImageType& input,
+ const float sigma_d,
+ const float sigma_i,
+ const float alpha,
+ const float thresh)
{
-
/*creating the gaussian kernels*/
ImageType kernel_d;
ImageType kernel_i;
}
}
-//////////////////////////////////////////////////////////////////////////////
+template <typename ImageType>
void
-pcl::keypoint::hessianBlob(ImageType& output,
- ImageType& input,
- const float sigma,
- bool SCALED)
+Keypoint<ImageType>::hessianBlob(ImageType& output,
+ ImageType& input,
+ const float sigma,
+ bool SCALED)
{
/*creating the gaussian kernels*/
ImageType kernel, cornerness;
}
}
-//////////////////////////////////////////////////////////////////////////////
+template <typename ImageType>
void
-pcl::keypoint::hessianBlob(ImageType& output,
- ImageType& input,
- const float start_scale,
- const float scaling_factor,
- const int num_scales)
+Keypoint<ImageType>::hessianBlob(ImageType& output,
+ ImageType& input,
+ const float start_scale,
+ const float scaling_factor,
+ const int num_scales)
{
const std::size_t height = input.size();
const std::size_t width = input[0].size();
hessianBlob(cornerness[i], input, scale, false);
scale *= scaling_factor;
}
- bool non_max_flag = false;
- float scale_max, local_max;
for (std::size_t i = 0; i < height; i++) {
for (std::size_t j = 0; j < width; j++) {
- scale_max = std::numeric_limits<float>::min();
+ float scale_max = std::numeric_limits<float>::min();
/*default output in case of no blob at the current point is 0*/
output[i][j] = 0;
for (int k = 0; k < num_scales; k++) {
/*check if the current point (k,i,j) is a maximum in the defined search radius*/
- non_max_flag = false;
- local_max = cornerness[k][i][j];
+ bool non_max_flag = false;
+ const float local_max = cornerness[k][i][j];
for (int n = -local_search_radius; n <= local_search_radius; n++) {
if (n + k < 0 || n + k >= num_scales)
continue;
scale_max = cornerness[k][i][j];
/*output indicates the scale at which the blob is found at the current
* location in the image*/
- output[i][i] = start_scale * pow(scaling_factor, k);
+ output[i][j] = start_scale * pow(scaling_factor, k);
}
}
}
}
}
-//////////////////////////////////////////////////////////////////////////////
+template <typename ImageType>
void
-pcl::keypoint::imageElementMultiply(ImageType& output,
- ImageType& input1,
- ImageType& input2)
+Keypoint<ImageType>::imageElementMultiply(ImageType& output,
+ ImageType& input1,
+ ImageType& input2)
{
const std::size_t height = input1.size();
const std::size_t width = input1[0].size();
}
}
}
-
-#endif // PCL_2D_KEYPOINT_HPP_
+} // namespace pcl
*
*/
-#ifndef PCL_2D_MORPHOLOGY_HPP_
-#define PCL_2D_MORPHOLOGY_HPP_
+#pragma once
+
+#include <pcl/2d/morphology.h>
+
+namespace pcl {
-//////////////////////////////////////////////////////////////////////////////
// Assumes input, kernel and output images have 0's and 1's only
template <typename PointT>
void
-
pcl::Morphology<PointT>::erosionBinary(pcl::PointCloud<PointT>& output)
+Morphology<PointT>::erosionBinary(pcl::PointCloud<PointT>& output)
{
const int height = input_->height;
const int width = input_->width;
}
}
-//////////////////////////////////////////////////////////////////////////////
// Assumes input, kernel and output images have 0's and 1's only
template <typename PointT>
void
-
pcl::Morphology<PointT>::dilationBinary(pcl::PointCloud<PointT>& output)
+Morphology<PointT>::dilationBinary(pcl::PointCloud<PointT>& output)
{
const int height = input_->height;
const int width = input_->width;
}
}
-//////////////////////////////////////////////////////////////////////////////
// Assumes input, kernel and output images have 0's and 1's only
template <typename PointT>
void
-
pcl::Morphology<PointT>::openingBinary(pcl::PointCloud<PointT>& output)
+Morphology<PointT>::openingBinary(pcl::PointCloud<PointT>& output)
{
PointCloudInPtr intermediate_output(new PointCloudIn);
erosionBinary(*intermediate_output);
dilationBinary(output);
}
-//////////////////////////////////////////////////////////////////////////////
// Assumes input, kernel and output images have 0's and 1's only
template <typename PointT>
void
-
pcl::Morphology<PointT>::closingBinary(pcl::PointCloud<PointT>& output)
+Morphology<PointT>::closingBinary(pcl::PointCloud<PointT>& output)
{
PointCloudInPtr intermediate_output(new PointCloudIn);
dilationBinary(*intermediate_output);
erosionBinary(output);
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::erosionGray(pcl::PointCloud<PointT>& output)
+Morphology<PointT>::erosionGray(pcl::PointCloud<PointT>& output)
{
const int height = input_->height;
const int width = input_->width;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::dilationGray(pcl::PointCloud<PointT>& output)
+Morphology<PointT>::dilationGray(pcl::PointCloud<PointT>& output)
{
const int height = input_->height;
const int width = input_->width;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::openingGray(pcl::PointCloud<PointT>& output)
+Morphology<PointT>::openingGray(pcl::PointCloud<PointT>& output)
{
PointCloudInPtr intermediate_output(new PointCloudIn);
erosionGray(*intermediate_output);
dilationGray(output);
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::closingGray(pcl::PointCloud<PointT>& output)
+Morphology<PointT>::closingGray(pcl::PointCloud<PointT>& output)
{
PointCloudInPtr intermediate_output(new PointCloudIn);
dilationGray(*intermediate_output);
erosionGray(output);
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::subtractionBinary(pcl::PointCloud<PointT>& output,
-
const pcl::PointCloud<PointT>& input1,
-
const pcl::PointCloud<PointT>& input2)
+Morphology<PointT>::subtractionBinary(pcl::PointCloud<PointT>& output,
+ const pcl::PointCloud<PointT>& input1,
+ const pcl::PointCloud<PointT>& input2)
{
const int height = (input1.height < input2.height) ? input1.height : input2.height;
const int width = (input1.width < input2.width) ? input1.width : input2.width;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::unionBinary(pcl::PointCloud<PointT>& output,
-
const pcl::PointCloud<PointT>& input1,
-
const pcl::PointCloud<PointT>& input2)
+Morphology<PointT>::unionBinary(pcl::PointCloud<PointT>& output,
+ const pcl::PointCloud<PointT>& input1,
+ const pcl::PointCloud<PointT>& input2)
{
const int height = (input1.height < input2.height) ? input1.height : input2.height;
const int width = (input1.width < input2.width) ? input1.width : input2.width;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::intersectionBinary(pcl::PointCloud<PointT>& output,
-
const pcl::PointCloud<PointT>& input1,
-
const pcl::PointCloud<PointT>& input2)
+Morphology<PointT>::intersectionBinary(pcl::PointCloud<PointT>& output,
+ const pcl::PointCloud<PointT>& input1,
+ const pcl::PointCloud<PointT>& input2)
{
const int height = (input1.height < input2.height) ? input1.height : input2.height;
const int width = (input1.width < input2.width) ? input1.width : input2.width;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::structuringElementCircular(pcl::PointCloud<PointT>& kernel,
- const int radius)
+Morphology<PointT>::structuringElementCircular(pcl::PointCloud<PointT>& kernel,
+ const int radius)
{
const int dim = 2 * radius;
kernel.height = dim;
}
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-
pcl::Morphology<PointT>::structuringElementRectangle(pcl::PointCloud<PointT>& kernel,
- const int height,
- const int width)
+Morphology<PointT>::structuringElementRectangle(pcl::PointCloud<PointT>& kernel,
+ const int height,
+ const int width)
{
kernel.height = height;
kernel.width = width;
kernel[i].intensity = 1;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
-pcl::Morphology<PointT>::setStructuringElement(
- const PointCloudInPtr& structuring_element)
+Morphology<PointT>::setStructuringElement(const PointCloudInPtr& structuring_element)
{
structuring_element_ = structuring_element;
}
-#endif // PCL_2D_MORPHOLOGY_HPP_
+} // namespace pcl
namespace pcl {
+template <typename ImageType>
class Keypoint {
private:
- Edge edge_detection;
- Convolution conv_2d;
+ Edge<ImageType, ImageType> edge_detection;
+ Convolution<ImageType> conv_2d;
public:
Keypoint() {}
* $Id$
*/
-#include <pcl/point_types.h>
-
#include <pcl/2d/convolution.h>
#include <pcl/2d/edge.h>
#include <pcl/2d/kernel.h>
#include <pcl/2d/morphology.h>
#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
using namespace pcl;
PCL is a large collaborative effort, and it would not exist without the contributions of several people.
Please see https://github.com/PointCloudLibrary/pcl/graphs/contributors for a complete list of developers.
-
# ChangeList
+## = 1.11.0 (11.05.2020) =
+
+Starting with PCL 1.11, PCL uses `std::shared_ptr` and `std::weak_ptr` instead of the
+boost smart pointers. The change leverages type aliases included with the 1.10.0
+release. PCL 1.11 also introduces `pcl::index_t` which should be used for the size
+of point types instead of `int`, `std::size_t`, etc. EOL for deprecated features
+is also explicitly mentioned in the deprecation compile time warnings
+
+### Notable changes
+
+**New features** *added to PCL*
+
+* **[common]** Provide dynamic and static pointer casts in namespace pcl to allow easy migration in future [[#3770](https://github.com/PointCloudLibrary/pcl/pull/3770)]
+* **[common]** Add `ignore` function to remove Doxygen warnings for unused arguments [[#3942](https://github.com/PointCloudLibrary/pcl/pull/3942)]
+* **[docs]** Generate TODO list [[#3937](https://github.com/PointCloudLibrary/pcl/pull/3937)]
+* Force include order via clang-format [[#3909](https://github.com/PointCloudLibrary/pcl/pull/3909)]
+* Change PCL smart pointers from `boost` to `std` [[#3750](https://github.com/PointCloudLibrary/pcl/pull/3750)]
+* **[ci]** Add pipeline for building PCL's environment docker image [[#3843](https://github.com/PointCloudLibrary/pcl/pull/3843)]
+
+**Deprecation** *of public APIs, scheduled to be removed after two minor releases*
+
+* **[common]** Remove `#undef Success` in pcl_macros.h by extracting `PCL_MAKE_ALIGNED_OPERATOR_NEW` into memory.h [[#3654](https://github.com/PointCloudLibrary/pcl/pull/3654)]
+* **[common]** Rename `point_traits.h` into `type_traits.h` [[#3698](https://github.com/PointCloudLibrary/pcl/pull/3698)]
+* **[filters]** Deprecating functions in non-speclialized Passthrough filter [[#3888](https://github.com/PointCloudLibrary/pcl/pull/3888)]
+* **[outofcore][registration]** Homogenize deprecation with PCL_DEPRECATED [[#3925](https://github.com/PointCloudLibrary/pcl/pull/3925)]
+* **[cmake][visualization]** Deprecate legacy OpenGL backend of VTK [[#4065](https://github.com/PointCloudLibrary/pcl/pull/4065)]
+
+**Removal** *of the public APIs deprecated in previous releases*
+
+* **[io][recognition][tools]** Remove very old deprecated headers [[#3906](https://github.com/PointCloudLibrary/pcl/pull/3906)]
+* **[docs]** Remove backup (and defunct) `CMakeLists.txt` [[#3915](https://github.com/PointCloudLibrary/pcl/pull/3915)]
+* Remove use of VTK_EXCLUDE_STRSTREAM_HEADERS (unavailable since VTK 6.0.0) [[#3939](https://github.com/PointCloudLibrary/pcl/pull/3939)]
+
+**Behavior changes** *in classes, apps, or tools*
+
+* **[io]** Make grabbers move-only using `unique_ptr` [[#3626](https://github.com/PointCloudLibrary/pcl/pull/3626)]
+
+**API changes** *that did not go through the proper deprecation and removal cycle*
+
+* **[io]** Make grabbers move-only using `unique_ptr` [[#3626](https://github.com/PointCloudLibrary/pcl/pull/3626)]
+* **[common]** Add `pcl::index_t`; move some type declarations from `pcl/pcl_macros.h` to `pcl/types.h` [[#3651](https://github.com/PointCloudLibrary/pcl/pull/3651)]
+* **[filters]** Clean up `Filter` and `FilterIndices`, move `indices_`/`input_` from public to protected section [[#3726](https://github.com/PointCloudLibrary/pcl/pull/3726)]
+* **[registration]** Better Generalized ICP optimizer gradient check management [[#3854](https://github.com/PointCloudLibrary/pcl/pull/3854)]
+* Change PCL smart pointers from `boost` to `std` [[#3750](https://github.com/PointCloudLibrary/pcl/pull/3750)]
+* **[registration]** Removing deprecated method `setInputCloud` from public API [[#4026](https://github.com/PointCloudLibrary/pcl/pull/4026)]
+
+**ABI changes** *that are still API compatible*
+
+* **[filters]** NormalSpaceSampling - fix bucket assignment, remove use of raw distribution pointer, unit-test rewriting [[#3862](https://github.com/PointCloudLibrary/pcl/pull/3862)]
+* **[io]** Add pcl::weak_ptr to have a single-switch move from boost:: to std:: weak pointers [[#3753](https://github.com/PointCloudLibrary/pcl/pull/3753)]
+
+### Changes grouped by module
+
+#### CMake:
+
+* Add a stamp file to build documentation once [[#3819](https://github.com/PointCloudLibrary/pcl/pull/3819)]
+* Fix compilation in OSX Catalina with OMP enabled [[#3721](https://github.com/PointCloudLibrary/pcl/pull/3721)]
+* Show proper message in CMake config for default-off modules [[#3927](https://github.com/PointCloudLibrary/pcl/pull/3927)]
+* **[deprecation]** Deprecate legacy OpenGL backend of VTK [[#4065](https://github.com/PointCloudLibrary/pcl/pull/4065)]
+
+#### libpcl_2d:
+
+* variable assigned a value which is never used [[#3857](https://github.com/PointCloudLibrary/pcl/pull/3857)]
+* Fix issue with missing templating of `Keypoint`. Fixes coming from clang-doxy [[#3898](https://github.com/PointCloudLibrary/pcl/pull/3898)]
+
+#### libpcl_common:
+
+* **[deprecation]** Remove `#undef Success` in pcl_macros.h by extracting `PCL_MAKE_ALIGNED_OPERATOR_NEW` into memory.h [[#3654](https://github.com/PointCloudLibrary/pcl/pull/3654)]
+* Fix issues with math defines on mingw-w64. [[#3756](https://github.com/PointCloudLibrary/pcl/pull/3756)]
+* **[API break]** Add `pcl::index_t`; move some type declarations from `pcl/pcl_macros.h` to `pcl/types.h` [[#3651](https://github.com/PointCloudLibrary/pcl/pull/3651)]
+* **[deprecation]** Rename `point_traits.h` into `type_traits.h` [[#3698](https://github.com/PointCloudLibrary/pcl/pull/3698)]
+* Improve `PCL_DEPRECATED` macro to include scheduled removal version [[#3808](https://github.com/PointCloudLibrary/pcl/pull/3808)]
+* Fix erroneous PCL version in deprecated message [[#3824](https://github.com/PointCloudLibrary/pcl/pull/3824)]
+* Select OpenMP data sharing mode based on specific GCC versions [[#3823](https://github.com/PointCloudLibrary/pcl/pull/3823)]
+* Define `PointIndices` based on the global `Indices` type alias [[#3822](https://github.com/PointCloudLibrary/pcl/pull/3822)]
+* Fix warning C4067: unexpected tokens following preprocessor directive- expected a newline [[#3871](https://github.com/PointCloudLibrary/pcl/pull/3871)]
+* **[new feature]** Provide dynamic and static pointer casts in namespace pcl to allow easy migration in future [[#3770](https://github.com/PointCloudLibrary/pcl/pull/3770)]
+* **[new feature]** Add `ignore` function to remove Doxygen warnings for unused arguments [[#3942](https://github.com/PointCloudLibrary/pcl/pull/3942)]
+* Fix excessive warnings on MSVC [[#3964](https://github.com/PointCloudLibrary/pcl/pull/3964)]
+* Refactoring `PCL_DEPRECATED` macro [[#3945](https://github.com/PointCloudLibrary/pcl/pull/3945)]
+* Correcting type mismatch [[#3967](https://github.com/PointCloudLibrary/pcl/pull/3967)]
+* Removed empty file [[#4019](https://github.com/PointCloudLibrary/pcl/pull/4019)]
+
+#### libpcl_filters:
+
+* Clean up code duplication in `FilterIndices` derived classes [[#3807](https://github.com/PointCloudLibrary/pcl/pull/3807)]
+* **[API break]** Clean up `Filter` and `FilterIndices`, move `indices_`/`input_` from public to protected section [[#3726](https://github.com/PointCloudLibrary/pcl/pull/3726)]
+* **[deprecation]** Deprecating functions in non-speclialized Passthrough filter [[#3888](https://github.com/PointCloudLibrary/pcl/pull/3888)]
+* **[ABI break]** NormalSpaceSampling - fix bucket assignment, remove use of raw distribution pointer, unit-test rewriting [[#3862](https://github.com/PointCloudLibrary/pcl/pull/3862)]
+* Optimize VoxelGrid Filter [[#3853](https://github.com/PointCloudLibrary/pcl/pull/3853)]
+* Fix error due to multiple declarations of template member function specializations in convolution [[#3971](https://github.com/PointCloudLibrary/pcl/pull/3971)]
+
+#### libpcl_io:
+
+* **[ABI break][API break][behavior change]** Make grabbers move-only using `unique_ptr` [[#3626](https://github.com/PointCloudLibrary/pcl/pull/3626)]
+* **[removal]** Remove very old deprecated headers [[#3906](https://github.com/PointCloudLibrary/pcl/pull/3906)]
+* **[ABI break]** Add pcl::weak_ptr to have a single-switch move from boost:: to std:: weak pointers [[#3753](https://github.com/PointCloudLibrary/pcl/pull/3753)]
+* Use pcl::io::raw_read instead of direct call to POSIX function read [[#4062](https://github.com/PointCloudLibrary/pcl/pull/4062)]
+
+#### libpcl_octree:
+
+* Fix a memory leak in `OctreeBase::operator=` [[#3787](https://github.com/PointCloudLibrary/pcl/pull/3787)]
+
+#### libpcl_outofcore:
+
+* **[deprecation]** Homogenize deprecation with PCL_DEPRECATED [[#3925](https://github.com/PointCloudLibrary/pcl/pull/3925)]
+
+#### libpcl_people:
+
+* Missing include on windows [[#3791](https://github.com/PointCloudLibrary/pcl/pull/3791)]
+
+#### libpcl_recognition:
+
+* **[removal]** Remove very old deprecated headers [[#3906](https://github.com/PointCloudLibrary/pcl/pull/3906)]
+
+#### libpcl_registration:
+
+* **[API break]** Better Generalized ICP optimizer gradient check management [[#3854](https://github.com/PointCloudLibrary/pcl/pull/3854)]
+* **[deprecation]** Homogenize deprecation with PCL_DEPRECATED [[#3925](https://github.com/PointCloudLibrary/pcl/pull/3925)]
+* **[API break]** Removing deprecated method `setInputCloud` from public API [[#4026](https://github.com/PointCloudLibrary/pcl/pull/4026)]
+
+#### libpcl_sample_consensus:
+
+* Better performance, error handling and other improvements in SAC classes [[#3642](https://github.com/PointCloudLibrary/pcl/pull/3642)]
+* Use better types for indices: `int` -> `index_t`, `std::vector<int>` ->`Indices` [[#3835](https://github.com/PointCloudLibrary/pcl/pull/3835)]
+
+#### libpcl_search:
+
+* Use better types for indices: `int` -> `index_t`, `std::vector<int>` ->`Indices` [[#3840](https://github.com/PointCloudLibrary/pcl/pull/3840)]
+* Add include for `pcl::isFinite` for compilation on VS2019 [[#4056](https://github.com/PointCloudLibrary/pcl/pull/4056)]
+
+#### libpcl_segmentation:
+
+* Check and warn user about missing normals in `OrganizedMultiPlaneSegmentation` [[#3861](https://github.com/PointCloudLibrary/pcl/pull/3861)]
+
+#### libpcl_surface:
+
+* Fix error due to multiple declarations of template member function specializations in Poisson4 [[#3972](https://github.com/PointCloudLibrary/pcl/pull/3972)]
+* Reduce time taken in `TextureMapping::sortFacesByCamera` from `O(faces*points)` to `O(faces)` [[#3981](https://github.com/PointCloudLibrary/pcl/pull/3981)]
+
+#### libpcl_visualization:
+
+* Minor fix for converting unorganized PointClouds to VTK data [[#3988](https://github.com/PointCloudLibrary/pcl/pull/3988)]
+* Fixes #4001 and #3452. [[#4017](https://github.com/PointCloudLibrary/pcl/pull/4017)]
+* **[deprecation]** Deprecate legacy OpenGL backend of VTK [[#4065](https://github.com/PointCloudLibrary/pcl/pull/4065)]
+* Fix rendering of points. [[#4067](https://github.com/PointCloudLibrary/pcl/pull/4067)]
+
+#### PCL Docs:
+
+* Fix Doxygen warnings unrelated to documentation -- Part 1 [[#3701](https://github.com/PointCloudLibrary/pcl/pull/3701)]
+* update automatic code formatting info to clang-format [[#3845](https://github.com/PointCloudLibrary/pcl/pull/3845)]
+* replace formula with math [[#3846](https://github.com/PointCloudLibrary/pcl/pull/3846)]
+* Fix PCL_DEPRECATED usage in doxygen for a proper Deprecation List in documentation [[#3905](https://github.com/PointCloudLibrary/pcl/pull/3905)]
+* **[removal]** Remove backup (and defunct) `CMakeLists.txt` [[#3915](https://github.com/PointCloudLibrary/pcl/pull/3915)]
+* **[new feature]** Generate TODO list [[#3937](https://github.com/PointCloudLibrary/pcl/pull/3937)]
+* Improve output to match the text in tutorial by adding a newline [[#4029](https://github.com/PointCloudLibrary/pcl/pull/4029)]
+* Fix typo in region growing tutorial [[#4052](https://github.com/PointCloudLibrary/pcl/pull/4052)]
+* Add information regarding header include order [[#4020](https://github.com/PointCloudLibrary/pcl/pull/4020)]
+
+#### PCL Tutorials:
+
+* Add a unit test for ICP and modernize tutorial code [[#3628](https://github.com/PointCloudLibrary/pcl/pull/3628)]
+
+#### PCL Examples:
+
+* Remove unnecessary includes in examples [[#4071](https://github.com/PointCloudLibrary/pcl/pull/4071)]
+
+#### PCL Tools:
+
+* **[removal]** Remove very old deprecated headers [[#3906](https://github.com/PointCloudLibrary/pcl/pull/3906)]
+
+#### CI:
+
+* Temporary fix for skipping of certain tests [[#3789](https://github.com/PointCloudLibrary/pcl/pull/3789)]
+* Simplify Ubuntu CI using matrix strategy [[#3783](https://github.com/PointCloudLibrary/pcl/pull/3783)]
+* Strip leading tag in commit message from changelog [[#3847](https://github.com/PointCloudLibrary/pcl/pull/3847)]
+* Shift to clang-format-10 to resolve bug in clang-format-{7-9} [[#3895](https://github.com/PointCloudLibrary/pcl/pull/3895)]
+* **[new feature]** Add pipeline for building PCL's environment docker image [[#3843](https://github.com/PointCloudLibrary/pcl/pull/3843)]
+* Improve docker ci for other PRs [[#4051](https://github.com/PointCloudLibrary/pcl/pull/4051)]
+* Remove unused variables from exceptions [[#4064](https://github.com/PointCloudLibrary/pcl/pull/4064)]
+* Removing hardcoded version number from tutorial CI [[#4058](https://github.com/PointCloudLibrary/pcl/pull/4058)]
+
## *= 1.10.1 (18.03.2020) =*
### Notable changes
set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "build type default to RelWithDebInfo, set to Release to improve performance" FORCE)
endif()
-project(PCL VERSION 1.10.1)
+project(PCL VERSION 1.11.0)
string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
### ---[ Find universal dependencies
set(CMAKE_MINSIZEREL_POSTFIX "s" CACHE STRING "Add postfix to target for MinSizeRel build")
endif()
-# ---[ special maintainer mode
-set(CMAKE_CXX_FLAGS_MAINTAINER "-pedantic -Wno-variadic-macros -Weffc++ -Wno-long-long" CACHE STRING
- "Flags used by the C++ compiler during maintainer builds."
- FORCE)
-set(CMAKE_C_FLAGS_MAINTAINER "-pedantic -Wno-variadic-macros -Weffc++ -Wno-long-long" CACHE STRING
- "Flags used by the C compiler during maintainer builds."
- FORCE)
-set(CMAKE_EXE_LINKER_FLAGS_MAINTAINER
- "-Wl,--warn-unresolved-symbols,--warn-once" CACHE STRING
- "Flags used for linking binaries during maintainer builds."
- FORCE)
-set(CMAKE_SHARED_LINKER_FLAGS_MAINTAINER
- "-Wl,--warn-unresolved-symbols,--warn-once" CACHE STRING
- "Flags used by the shared libraries linker during maintainer builds."
- FORCE)
-mark_as_advanced(
- CMAKE_CXX_FLAGS_MAINTAINER
- CMAKE_C_FLAGS_MAINTAINER
- CMAKE_EXE_LINKER_FLAGS_MAINTAINER
- CMAKE_SHARED_LINKER_FLAGS_MAINTAINER)
# Update the documentation string of CMAKE_BUILD_TYPE for GUIs
set(CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" CACHE STRING
- "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel Maintainer."
+ "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
FORCE)
# Compiler identification
endif()
### ---[ Find universal dependencies
-find_package(OpenMP)
-if(OPENMP_FOUND)
+
+# OpenMP (optional)
+find_package(OpenMP COMPONENTS C CXX)
+if(OpenMP_FOUND)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
- message(STATUS "Found OpenMP")
+ if(${CMAKE_VERSION} VERSION_LESS "3.7")
+ message(STATUS "Found OpenMP")
+ else()
+ # We could use OpenMP_CXX_VERSION starting from CMake 3.9, but this value is only available on first run of CMake (see https://gitlab.kitware.com/cmake/cmake/issues/19150),
+ # so we use always OpenMP_CXX_SPEC_DATE, which is available since CMake 3.7.
+ message(STATUS "Found OpenMP, spec date ${OpenMP_CXX_SPEC_DATE}")
+ endif()
if(MSVC)
if(MSVC_VERSION EQUAL 1900)
set(OPENMP_DLL VCOMP140)
endif()
if(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL")
set(VTK_RENDERING_BACKEND_OPENGL_VERSION "1")
+ message(DEPRECATION "The rendering backend OpenGL is deprecated and not available anymore since VTK 8.2."
+ "Please switch to the OpenGL2 backend instead, which is available since VTK 6.2."
+ "Support of the deprecated backend will be dropped with PCL 1.13.")
elseif(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL2")
set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2")
endif()
PCL_ADD_INCLUDES(common "" "${pcl_config_h}")
include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
-### ---[ Add the libraries subdirectories
-include("${PCL_SOURCE_DIR}/cmake/pcl_targets.cmake")
-
collect_subproject_directory_names("${PCL_SOURCE_DIR}" "CMakeLists.txt" PCL_MODULES_NAMES PCL_MODULES_DIRS doc)
set(PCL_MODULES_NAMES_UNSORTED ${PCL_MODULES_NAMES})
topological_sort(PCL_MODULES_NAMES PCL_ _DEPENDS)
* [Stack Overflow](https://stackoverflow.com/questions/tagged/point-cloud-library)
for Q&A as well as support for troubleshooting, installation and debugging. Do
remember to tag your questions with the tag `point-cloud-library`.
- * [Gitter channel](https://gitter.im/PointCloudLibrary/pcl) for live chat with
+ * [Discord Server](https://discord.gg/JFFMAXS) for live chat with
other members of the PCL community and casual discussions
<!--
# Point Cloud Library
-<img src="pcl.png" align="center" height="100">
+<p align="center"><img src="pcl.png" height="100"></p>
[![Release][release-image]][releases]
[![License][license-image]][license]
-[release-image]: https://img.shields.io/badge/release-1.10.1-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-1.11.0-green.svg?style=flat
[releases]: https://github.com/PointCloudLibrary/pcl/releases
[license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
[license]: https://github.com/PointCloudLibrary/pcl/blob/master/LICENSE.txt
-Continuous integration
-----------------------
+:bangbang:Website:bangbang:
+-------
-[![Ubuntu Build Status][ci-ubuntu-image]][ci-ubuntu] [![Windows Build Status][ci-windows-image]][ci-windows] [![MacOS Build Status][ci-macos-image]][ci-macos]
+The original website (http://pointclouds.org) is down currently :broken_heart:, but a new one is back up https://pointcloudlibrary.github.io/ :heart: and open to [contributions](https://github.com/PointCloudLibrary/PointCloudLibrary.github.io) :hammer_and_wrench:.
-[ci-ubuntu]: https://dev.azure.com/PointCloudLibrary/pcl/_build/latest?definitionId=4
-[ci-ubuntu-image]: https://img.shields.io/azure-devops/build/PointCloudLibrary/0fc52e87-00b9-420e-acbc-c842c4f2d9cc/4.svg?label=Ubuntu&logo=azure%20pipelines
-[ci-windows]: https://dev.azure.com/PointCloudLibrary/pcl/_build/latest?definitionId=3
-[ci-windows-image]: https://img.shields.io/azure-devops/build/PointCloudLibrary/0fc52e87-00b9-420e-acbc-c842c4f2d9cc/3.svg?label=Windows&logo=azure%20pipelines
-[ci-macos]: https://dev.azure.com/PointCloudLibrary/pcl/_build/latest?definitionId=2
-[ci-macos-image]: https://img.shields.io/azure-devops/build/PointCloudLibrary/0fc52e87-00b9-420e-acbc-c842c4f2d9cc/2.svg?label=MacOS&logo=azure%20pipelines
+If you really need access to the old website, please use [the copy made by the internet archive](https://web.archive.org/web/20191017164724/http://www.pointclouds.org/). Please be aware that the website was hacked before and could still be hosting some malicious code.
+
+Continuous integration
+----------------------
+[ci-latest-build]: https://dev.azure.com/PointCloudLibrary/pcl/_build/latest?definitionId=9&branchName=master
+[ci-ubuntu-16.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Ubuntu&jobName=Ubuntu&configuration=Ubuntu%2016.04%20GCC&label=Ubuntu%2016.04
+[ci-ubuntu-20.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Ubuntu&jobName=Ubuntu&configuration=Ubuntu%2020.04%20GCC&label=Ubuntu%2020.04
+[ci-windows-x86]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Windows&jobName=Windows%20VS2017%20Build&configuration=Windows%20VS2017%20Build%20x86&label=Windows%20VS2017%20x86
+[ci-windows-x64]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Windows&jobName=Windows%20VS2017%20Build&configuration=Windows%20VS2017%20Build%20x64&label=Windows%20VS2017%20x64
+[ci-macos-10.14]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20macOS&jobName=macOS&configuration=macOS%20Mojave%2010.14&label=macOS%20Mojave%2010.14
+[ci-macos-10.15]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20macOS&jobName=macOS&configuration=macOS%20Catalina%2010.15&label=macOS%20Catalina%2010.15
+
+Build Platform | Status
+------------------------ | ------------------------------------------------------------------------------------------------- |
+Ubuntu | [![Status][ci-ubuntu-16.04]][ci-latest-build] <br> [![Status][ci-ubuntu-20.04]][ci-latest-build] |
+Windows | [![Status][ci-windows-x86]][ci-latest-build] <br> [![Status][ci-windows-x64]][ci-latest-build] |
+macOS | [![Status][ci-macos-10.14]][ci-latest-build] <br> [![Status][ci-macos-10.15]][ci-latest-build] |
Community
---------
-[![Gitter][gitter-image]][gitter-channel]
+[![Discord][discord-image]][discord-server]
[![StackOverflow][so-question-count]][stackoverflow]
[![Website][website-status]][website]
-[gitter-image]: https://img.shields.io/gitter/room/PointCloudLibrary/pcl.svg?label=community%20chat&logo=gitter&logoColor=%23ED1965
-[gitter-channel]: https://gitter.im/PointCloudLibrary/pcl
-[website-status]: https://img.shields.io/website/http/www.pointclouds.org.svg?down_color=red&down_message=is%20down&up_color=yellow&up_message=is%20outdated
-[website]: http://www.pointclouds.org
+
+[discord-image]: https://img.shields.io/discord/694824801977630762?color=7289da&label=community%20chat&logo=discord&style=plastic
+[discord-server]: https://discord.gg/JFFMAXS
+[website-status]: https://img.shields.io/website/https/pointcloudlibrary.github.io.svg?down_color=red&down_message=is%20down&up_color=green&up_message=is%20new
+[website]: https://pointcloudlibrary.github.io/
[so-question-count]: https://img.shields.io/stackexchange/stackoverflow/t/point-cloud-library.svg?logo=stackoverflow
[stackoverflow]: https://stackoverflow.com/questions/tagged/point-cloud-library
Distribution
---------
[](https://repology.org/project/pcl-pointclouds/badges)
+[](https://repology.org/project/pcl-pointclouds/versions)
+
<details>
<summary>Click to see all</summary>
<p>
-<a href="https://repology.org/project/pcl-pointclouds/versions"><img src="https://repology.org/badge/vertical-allrepos/pcl-pointclouds.svg?header=pcl-pointclouds"></a>
+<a href="https://repology.org/project/pcl-pointclouds/packages">
+ <img src="https://repology.org/badge/vertical-allrepos/pcl-pointclouds.svg?columns=3"
+ alt="Packaging status">
+</a>
</p>
</details>
* [Stack Overflow](https://stackoverflow.com/questions/tagged/point-cloud-library)
for Q&A as well as support for troubleshooting, installation and debugging. Do
remember to tag your questions with the tag `point-cloud-library`.
-* [Gitter channel](https://gitter.im/PointCloudLibrary/pcl) for live chat with
+* [Discord Server](https://discord.gg/JFFMAXS) for live chat with
other members of the PCL community and casual discussions
<!--
#include <pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/common/time.h>
+#include <pcl/memory.h> // for pcl::make_shared
namespace pcl
{
computeMeshResolution (PointInTPtr & input)
{
using KdTreeInPtr = typename pcl::KdTree<PointInT>::Ptr;
- KdTreeInPtr tree =
boost::make_shared<pcl::KdTreeFLANN<PointInT> > (false);
+ KdTreeInPtr tree =
pcl::make_shared<pcl::KdTreeFLANN<PointInT> > (false);
tree->setInputCloud (input);
std::vector<int> nn_indices (9);
voxel_grid_icp.filter (*cloud_voxelized_icp);
source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
-#pragma omp parallel for num_threads(omp_get_num_procs())
+#pragma omp parallel for \
+ default(none) \
+ shared(VOXEL_SIZE_ICP_, cloud_voxelized_icp) \
+ num_threads(omp_get_num_procs())
for (int i = 0; i < static_cast<int> (models_->size ()); i++)
{
source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
}
-#pragma omp parallel for num_threads(omp_get_num_procs())
+#pragma omp parallel for \
+ default(none) \
+ shared(cloud_voxelized_icp, VOXEL_SIZE_ICP_) \
+ num_threads(omp_get_num_procs())
for (int i = 0; i < static_cast<int> (models_->size ()); i++)
{
} else {
models = source_->getModels (search_model_);
//reset cache and flann structures
- if(flann_index_ != nullptr)
delete flann_index_;
flann_models_.clear();
voxel_grid_icp.filter (*cloud_voxelized_icp);
source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
-#pragma omp parallel for schedule(dynamic,1) num_threads(omp_get_num_procs())
+#pragma omp parallel for \
+ default(none) \
+ shared(cloud_voxelized_icp) \
+ schedule(dynamic,1) \
+ num_threads(omp_get_num_procs())
for (int i = 0; i < static_cast<int>(models_->size ()); i++)
{
const PointCloudPtr
snap ();
bool
- isActive ();
+ isActive () const;
void
onKeyboardEvent (const pcl::visualization::KeyboardEvent & event);
#include "pcl/apps/3d_rec_framework/tools/openni_frame_source.h"
#include <pcl/io/pcd_io.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
namespace OpenNIFrameSource
{
}
bool
- OpenNIFrameSource::isActive ()
+ OpenNIFrameSource::isActive () const
{
return active_;
}
set(SUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition ml stereo 2d)
set(DEFAULT FALSE)
+set(REASON "Disabled by default")
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni vtk)
target_link_libraries(pcl_feature_matching pcl_common pcl_io pcl_registration pcl_keypoints pcl_sample_consensus pcl_visualization pcl_search pcl_features pcl_kdtree pcl_surface pcl_segmentation)
PCL_ADD_EXECUTABLE(pcl_convolve COMPONENT ${SUBSYS_NAME} SOURCES src/convolve.cpp)
- target_link_libraries(pcl_convolve pcl_common pcl_io pcl_visualization)
+ target_link_libraries(pcl_convolve pcl_common pcl_filters pcl_io pcl_visualization)
PCL_ADD_EXECUTABLE(pcl_pcd_organized_multi_plane_segmentation COMPONENT ${SUBSYS_NAME} SOURCES src/pcd_organized_multi_plane_segmentation.cpp)
target_link_libraries(pcl_pcd_organized_multi_plane_segmentation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_features)
endif()
add_custom_target(${SUBSYS_TARGET_NAME} DEPENDS ${PCL_APPS_ALL_TARGETS})
set_target_properties(${SUBSYS_TARGET_NAME} PROPERTIES FOLDER "Apps")
-
#define IMPL_CLOUD_ITEM_H_
#include <pcl/apps/cloud_composer/items/cloud_item.h>
+#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_cloud.h>
#include <pcl/impl/instantiate.hpp>
template <typename PointT> pcl::cloud_composer::CloudItem*
pcl::cloud_composer::CloudItem::createCloudItemFromTemplate (const QString& name, typename PointCloud<PointT>::Ptr cloud_ptr)
{
- pcl::PCLPointCloud2::Ptr cloud_blob =
boost::make_shared <pcl::PCLPointCloud2> ();
+ pcl::PCLPointCloud2::Ptr cloud_blob =
pcl::make_shared <pcl::PCLPointCloud2> ();
toPCLPointCloud2 (*cloud_ptr, *cloud_blob);
CloudItem* cloud_item = new CloudItem ( name, cloud_blob, Eigen::Vector4f (), Eigen::Quaternionf (), false);
cloud_item->setData (QVariant::fromValue(cloud_ptr), ItemDataRole::CLOUD_TEMPLATED);
#include <QDebug>
#include <pcl/apps/cloud_composer/items/cloud_composer_item.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/search/kdtree.h>
- /*
- * Software License Agreement (BSD License)
- *
- * Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2012, Jeremie Papon.
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- */
-
- #ifndef IMPL_ORGANIZED_SEGMENTATION_HPP_
- #define IMPL_ORGANIZED_SEGMENTATION_HPP_
-
-
#include <pcl/apps/cloud_composer/tools/organized_segmentation.h>
-
#include <pcl/apps/cloud_composer/impl/cloud_item.hpp>
-
#include <pcl/apps/cloud_composer/items/normals_item.h>
- #include <pcl/point_cloud.h>
-
- #include <pcl/segmentation/organized_multi_plane_segmentation.h>
- #include <pcl/segmentation/plane_coefficient_comparator.h>
- #include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
- #include <pcl/segmentation/rgb_plane_coefficient_comparator.h>
- #include <pcl/segmentation/edge_aware_plane_comparator.h>
- #include <pcl/segmentation/euclidean_cluster_comparator.h>
- #include <pcl/segmentation/organized_connected_component_segmentation.h>
-
-
- template <typename PointT> QList <pcl::cloud_composer::CloudComposerItem*>
- pcl::cloud_composer::OrganizedSegmentationTool::performTemplatedAction (const QList <const CloudComposerItem*>& input_data)
- {
- QList <CloudComposerItem*> output;
-
- foreach (const CloudComposerItem* input_item, input_data)
- {
- QVariant variant = input_item->data (ItemDataRole::CLOUD_TEMPLATED);
- if ( ! variant.canConvert <typename PointCloud<PointT>::Ptr> () )
- {
- qWarning () << "Attempted to cast to template type which does not exist in this item! (input list)";
- return output;
- }
- typename PointCloud <PointT>::Ptr input_cloud = variant.value <typename PointCloud<PointT>::Ptr> ();
- if ( ! input_cloud->isOrganized ())
- {
- qCritical () << "Organized Segmentation requires an organized cloud!";
- return output;
- }
- }
-
- int min_inliers = parameter_model_->getProperty ("Min Inliers").toInt ();
- int min_plane_size = parameter_model_->getProperty ("Min Plane Size").toInt ();
- double angular_threshold = parameter_model_->getProperty ("Angular Threshold").toDouble ();
- double distance_threshold = parameter_model_->getProperty ("Distance Threshold").toDouble ();
- double cluster_distance_threshold = parameter_model_->getProperty ("Cluster Dist. Thresh.").toDouble ();
- int min_cluster_size = parameter_model_->getProperty ("Min Cluster Size").toInt ();
-
- foreach (const CloudComposerItem* input_item, input_data)
- {
- QList <CloudComposerItem*> normals_list = input_item->getChildren (CloudComposerItem::NORMALS_ITEM);
- //Get the normals cloud, we just use the first normals that were found if there are more than one
- pcl::PointCloud<pcl::Normal>::ConstPtr input_normals = normals_list.value(0)->data(ItemDataRole::CLOUD_TEMPLATED).value <pcl::PointCloud<pcl::Normal>::ConstPtr> ();
-
- QVariant variant = input_item->data (ItemDataRole::CLOUD_TEMPLATED);
- typename PointCloud <PointT>::Ptr input_cloud = variant.value <typename PointCloud<PointT>::Ptr> ();
-
- pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
- mps.setMinInliers (min_inliers);
- mps.setAngularThreshold (pcl::deg2rad (angular_threshold)); // convert to radians
- mps.setDistanceThreshold (distance_threshold);
- mps.setInputNormals (input_normals);
- mps.setInputCloud (input_cloud);
- std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
- std::vector<pcl::ModelCoefficients> model_coefficients;
- std::vector<pcl::PointIndices> inlier_indices;
- pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
- std::vector<pcl::PointIndices> label_indices;
- std::vector<pcl::PointIndices> boundary_indices;
- mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
-
- auto plane_labels = boost::make_shared<std::set<std::uint32_t> > ();
- for (std::size_t i = 0; i < label_indices.size (); ++i)
- if (label_indices[i].indices.size () > (std::size_t) min_plane_size)
- plane_labels->insert (i);
- typename PointCloud<PointT>::CloudVectorType clusters;
-
- typename EuclideanClusterComparator<PointT, pcl::Label>::Ptr euclidean_cluster_comparator(new EuclideanClusterComparator<PointT, pcl::Label>);
- euclidean_cluster_comparator->setInputCloud (input_cloud);
- euclidean_cluster_comparator->setLabels (labels);
- euclidean_cluster_comparator->setExcludeLabels (plane_labels);
- euclidean_cluster_comparator->setDistanceThreshold (cluster_distance_threshold, false);
-
- pcl::PointCloud<pcl::Label> euclidean_labels;
- std::vector<pcl::PointIndices> euclidean_label_indices;
- pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator);
- euclidean_segmentation.setInputCloud (input_cloud);
- euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
-
- pcl::IndicesPtr extracted_indices (new std::vector<int> ());
- for (std::size_t i = 0; i < euclidean_label_indices.size (); i++)
- {
- if (euclidean_label_indices[i].indices.size () >= (std::size_t) min_cluster_size)
- {
- typename PointCloud<PointT>::Ptr cluster (new PointCloud<PointT>);
- pcl::copyPointCloud (*input_cloud,euclidean_label_indices[i].indices,*cluster);
- qDebug () << "Found cluster with size " << cluster->width;
- QString name = input_item->text () + tr ("- Clstr %1").arg (i);
- CloudItem* new_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(name,cluster);
- output.append (new_cloud_item);
- extracted_indices->insert (extracted_indices->end (), euclidean_label_indices[i].indices.begin (), euclidean_label_indices[i].indices.end ());
-
- }
- }
-
- for (std::size_t i = 0; i < label_indices.size (); i++)
- {
- if (label_indices[i].indices.size () >= (std::size_t) min_plane_size)
- {
-
typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
- pcl::copyPointCloud (*input_cloud,label_indices[i].indices,*plane);
- qDebug () << "Found plane with size " << plane->width;
- QString name = input_item->text () + tr ("- Plane %1").arg (i);
-
CloudItem* new_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(name,plane);
- output.append (new_cloud_item);
- extracted_indices->insert (extracted_indices->end (), label_indices[i].indices.begin (), label_indices[i].indices.end ());
-
- }
- }
-
typename PointCloud<PointT>::Ptr leftovers (new PointCloud<PointT>);
- if (extracted_indices->empty ())
- pcl::copyPointCloud (*input_cloud,*leftovers);
- else
- {
-
pcl::ExtractIndices<PointT> filter;
- filter.setInputCloud (input_cloud);
- filter.setIndices (extracted_indices);
- filter.setNegative (true);
- filter.filter (*leftovers);
- }
-
CloudItem* leftover_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(input_item->text(),leftovers);
- output.append (leftover_cloud_item);
- }
-
-
-
-
- return output;
-
- }
-
-
- #define PCL_INSTANTIATE_performTemplatedAction(T) template void pcl::cloud_composer::OrganizedSegmentationTool::performTemplatedAction<T> (QList <const CloudComposerItem*>);
-
-
-
- #endif //IMPL_TRANSFORM_CLOUDS_HPP_
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012, Jeremie Papon.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef IMPL_ORGANIZED_SEGMENTATION_HPP_
+#define IMPL_ORGANIZED_SEGMENTATION_HPP_
+
+#include <pcl/apps/cloud_composer/tools/organized_segmentation.h>
+#include <pcl/apps/cloud_composer/impl/cloud_item.hpp>
+#include <pcl/apps/cloud_composer/items/normals_item.h>
+#include <pcl/memory.h> // for pcl::make_shared
+#include <pcl/point_cloud.h>
+
+#include <pcl/segmentation/organized_multi_plane_segmentation.h>
+#include <pcl/segmentation/plane_coefficient_comparator.h>
+#include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
+#include <pcl/segmentation/rgb_plane_coefficient_comparator.h>
+#include <pcl/segmentation/edge_aware_plane_comparator.h>
+#include <pcl/segmentation/euclidean_cluster_comparator.h>
+#include <pcl/segmentation/organized_connected_component_segmentation.h>
+
+
+template <typename PointT> QList <pcl::cloud_composer::CloudComposerItem*>
+pcl::cloud_composer::OrganizedSegmentationTool::performTemplatedAction (const QList <const CloudComposerItem*>& input_data)
+{
+ QList <CloudComposerItem*> output;
+
+ foreach (const CloudComposerItem* input_item, input_data)
+ {
+ QVariant variant = input_item->data (ItemDataRole::CLOUD_TEMPLATED);
+ if ( ! variant.canConvert <typename PointCloud<PointT>::Ptr> () )
+ {
+ qWarning () << "Attempted to cast to template type which does not exist in this item! (input list)";
+ return output;
+ }
+ typename PointCloud <PointT>::Ptr input_cloud = variant.value <typename PointCloud<PointT>::Ptr> ();
+ if ( ! input_cloud->isOrganized ())
+ {
+ qCritical () << "Organized Segmentation requires an organized cloud!";
+ return output;
+ }
+ }
+
+ int min_inliers = parameter_model_->getProperty ("Min Inliers").toInt ();
+ int min_plane_size = parameter_model_->getProperty ("Min Plane Size").toInt ();
+ double angular_threshold = parameter_model_->getProperty ("Angular Threshold").toDouble ();
+ double distance_threshold = parameter_model_->getProperty ("Distance Threshold").toDouble ();
+ double cluster_distance_threshold = parameter_model_->getProperty ("Cluster Dist. Thresh.").toDouble ();
+ int min_cluster_size = parameter_model_->getProperty ("Min Cluster Size").toInt ();
+
+ foreach (const CloudComposerItem* input_item, input_data)
+ {
+ QList <CloudComposerItem*> normals_list = input_item->getChildren (CloudComposerItem::NORMALS_ITEM);
+ //Get the normals cloud, we just use the first normals that were found if there are more than one
+ pcl::PointCloud<pcl::Normal>::ConstPtr input_normals = normals_list.value(0)->data(ItemDataRole::CLOUD_TEMPLATED).value <pcl::PointCloud<pcl::Normal>::ConstPtr> ();
+
+ QVariant variant = input_item->data (ItemDataRole::CLOUD_TEMPLATED);
+ typename PointCloud <PointT>::Ptr input_cloud = variant.value <typename PointCloud<PointT>::Ptr> ();
+
+ pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
+ mps.setMinInliers (min_inliers);
+ mps.setAngularThreshold (pcl::deg2rad (angular_threshold)); // convert to radians
+ mps.setDistanceThreshold (distance_threshold);
+ mps.setInputNormals (input_normals);
+ mps.setInputCloud (input_cloud);
+ std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
+ std::vector<pcl::ModelCoefficients> model_coefficients;
+ std::vector<pcl::PointIndices> inlier_indices;
+ pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
+ std::vector<pcl::PointIndices> label_indices;
+ std::vector<pcl::PointIndices> boundary_indices;
+ mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
+
+ auto plane_labels = pcl::make_shared<std::set<std::uint32_t> > ();
+ for (std::size_t i = 0; i < label_indices.size (); ++i)
+ if (label_indices[i].indices.size () > (std::size_t) min_plane_size)
+ plane_labels->insert (i);
+ typename PointCloud<PointT>::CloudVectorType clusters;
+
+ typename EuclideanClusterComparator<PointT, pcl::Label>::Ptr euclidean_cluster_comparator(new EuclideanClusterComparator<PointT, pcl::Label>);
+ euclidean_cluster_comparator->setInputCloud (input_cloud);
+ euclidean_cluster_comparator->setLabels (labels);
+ euclidean_cluster_comparator->setExcludeLabels (plane_labels);
+ euclidean_cluster_comparator->setDistanceThreshold (cluster_distance_threshold, false);
+
+ pcl::PointCloud<pcl::Label> euclidean_labels;
+ std::vector<pcl::PointIndices> euclidean_label_indices;
+ pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator);
+ euclidean_segmentation.setInputCloud (input_cloud);
+ euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
+
+ pcl::IndicesPtr extracted_indices (new std::vector<int> ());
+ for (std::size_t i = 0; i < euclidean_label_indices.size (); i++)
+ {
+ if (euclidean_label_indices[i].indices.size () >= (std::size_t) min_cluster_size)
+ {
+ typename PointCloud<PointT>::Ptr cluster (new PointCloud<PointT>);
+ pcl::copyPointCloud (*input_cloud,euclidean_label_indices[i].indices,*cluster);
+ qDebug () << "Found cluster with size " << cluster->width;
+ QString name = input_item->text () + tr ("- Clstr %1").arg (i);
+ CloudItem* new_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(name,cluster);
+ output.append (new_cloud_item);
+ extracted_indices->insert (extracted_indices->end (), euclidean_label_indices[i].indices.begin (), euclidean_label_indices[i].indices.end ());
+ }
+ }
+
+ for (std::size_t i = 0; i < label_indices.size (); i++)
+ {
+ if (label_indices[i].indices.size () >= (std::size_t) min_plane_size)
+ {
+ typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
+ pcl::copyPointCloud (*input_cloud,label_indices[i].indices,*plane);
+ qDebug () << "Found plane with size " << plane->width;
+ QString name = input_item->text () + tr ("- Plane %1").arg (i);
+ CloudItem* new_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(name,plane);
+ output.append (new_cloud_item);
+ extracted_indices->insert (extracted_indices->end (), label_indices[i].indices.begin (), label_indices[i].indices.end ());
+
+ }
+ }
+ typename PointCloud<PointT>::Ptr leftovers (new PointCloud<PointT>);
+ if (extracted_indices->empty ())
+ pcl::copyPointCloud (*input_cloud,*leftovers);
+ else
+ {
+ pcl::ExtractIndices<PointT> filter;
+ filter.setInputCloud (input_cloud);
+ filter.setIndices (extracted_indices);
+ filter.setNegative (true);
+ filter.filter (*leftovers);
+ }
+ CloudItem* leftover_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(input_item->text(),leftovers);
+ output.append (leftover_cloud_item);
+ }
+
+
+
+
+ return output;
+
+}
+
+
+#define PCL_INSTANTIATE_performTemplatedAction(T) template void pcl::cloud_composer::OrganizedSegmentationTool::performTemplatedAction<T> (QList <const CloudComposerItem*>);
+
+
+
+#endif //IMPL_TRANSFORM_CLOUDS_HPP_
#include <pcl/apps/cloud_composer/tools/supervoxels.h>
#include <pcl/apps/cloud_composer/impl/cloud_item.hpp>
#include <pcl/apps/cloud_composer/items/normals_item.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <pcl/point_cloud.h>
#include <pcl/segmentation/supervoxel_clustering.h>
QList <QStandardItem*> items_to_remove = removed_to_parent_map_.keys ();
foreach (QStandardItem* to_remove, items_to_remove)
{
- if (to_remove)
delete to_remove;
}
}
{
QList <CloudComposerItem*> new_items = output_pair.output_items_;
foreach (CloudComposerItem* item, new_items)
- if (item)
- delete item;
+ {
+ delete item;
+ }
}
-
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include <pcl/apps/cloud_composer/items/cloud_item.h>
#include <pcl/filters/passthrough.h>
+#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/impl/instantiate.hpp>
#include <pcl/apps/cloud_composer/impl/cloud_item.hpp>
pcl::fromPCLPointCloud2 (*cloud_blob_ptr_, *cloud_ptr);
cloud_pointer_variant = QVariant::fromValue (cloud_ptr);
//Initialize the search kd-tree for this cloud
- pcl::search::KdTree<PointXYZ>::Ptr kd_search =
boost::make_shared<search::KdTree<PointXYZ> >();
+ pcl::search::KdTree<PointXYZ>::Ptr kd_search =
pcl::make_shared<search::KdTree<PointXYZ> >();
kd_search->setInputCloud (cloud_ptr);
kd_tree_variant = QVariant::fromValue (kd_search);
break;
pcl::PointCloud <PointXYZRGB>::Ptr cloud_ptr (new pcl::PointCloud <PointXYZRGB>);
pcl::fromPCLPointCloud2 (*cloud_blob_ptr_, *cloud_ptr);
cloud_pointer_variant = QVariant::fromValue (cloud_ptr);
- pcl::search::KdTree<PointXYZRGB>::Ptr kd_search =
boost::make_shared<search::KdTree<PointXYZRGB> >();
+ pcl::search::KdTree<PointXYZRGB>::Ptr kd_search =
pcl::make_shared<search::KdTree<PointXYZRGB> >();
kd_search->setInputCloud (cloud_ptr);
kd_tree_variant = QVariant::fromValue (kd_search);
break;
pcl::PointCloud <PointXYZRGBA>::Ptr cloud_ptr (new pcl::PointCloud <PointXYZRGBA>);
pcl::fromPCLPointCloud2 (*cloud_blob_ptr_, *cloud_ptr);
cloud_pointer_variant = QVariant::fromValue (cloud_ptr);
- pcl::search::KdTree<PointXYZRGBA>::Ptr kd_search =
boost::make_shared<search::KdTree<PointXYZRGBA> >();
+ pcl::search::KdTree<PointXYZRGBA>::Ptr kd_search =
pcl::make_shared<search::KdTree<PointXYZRGBA> >();
kd_search->setInputCloud (cloud_ptr);
kd_tree_variant = QVariant::fromValue (kd_search);
break;
if (! cloud_blob_ptr_)
return false;
- pcl::PCLPointCloud2::Ptr cloud_filtered =
boost::make_shared<pcl::PCLPointCloud2> ();
+ pcl::PCLPointCloud2::Ptr cloud_filtered =
pcl::make_shared<pcl::PCLPointCloud2> ();
pcl::PassThrough<pcl::PCLPointCloud2> pass_filter;
pass_filter.setInputCloud (cloud_blob_ptr_);
pass_filter.setKeepOrganized (false);
#include <pcl/apps/cloud_composer/items/cloud_item.h>
#include <pcl/filters/extract_indices.h>
+#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/impl/instantiate.hpp>
#include <pcl/apps/cloud_composer/impl/merge_selection.hpp>
: MergeCloudTool (nullptr, parent)
, selected_item_index_map_ (std::move(selected_item_index_map))
{
-
+
}
pcl::cloud_composer::MergeSelection::~MergeSelection ()
{
-
+
}
QList <pcl::cloud_composer::CloudComposerItem*>
return this->performTemplatedAction<pcl::PointXYZRGBA> (input_data);
}
}
-
+
QList <CloudComposerItem*> output;
// Check input data length
bool pose_found = false;
foreach (const CloudItem* input_cloud_item, selected_item_index_map_.keys ())
{
- //If this cloud hasn't been completely selected
+ //If this cloud hasn't been completely selected
if (!input_data.contains (input_cloud_item))
{
pcl::PCLPointCloud2::ConstPtr input_cloud = input_cloud_item->data (ItemDataRole::CLOUD_BLOB).value <pcl::PCLPointCloud2::ConstPtr> ();
qDebug () << "Extracting "<<selected_item_index_map_.value(input_cloud_item)->indices.size() << " points out of "<<input_cloud->width;
filter.setInputCloud (input_cloud);
filter.setIndices (selected_item_index_map_.value (input_cloud_item));
- pcl::PCLPointCloud2::Ptr original_minus_indices =
boost::make_shared <pcl::PCLPointCloud2> ();
+ pcl::PCLPointCloud2::Ptr original_minus_indices =
pcl::make_shared <pcl::PCLPointCloud2> ();
filter.setNegative (true);
filter.filter (*original_minus_indices);
filter.setNegative (false);
pcl::PCLPointCloud2::Ptr selected_points (new pcl::PCLPointCloud2);
filter.filter (*selected_points);
-
+
qDebug () << "Original minus indices is "<<original_minus_indices->width;
if (!pose_found)
, source_origin
, source_orientation);
output.append (new_cloud_item);
- pcl::PCLPointCloud2::Ptr temp_cloud =
boost::make_shared <pcl::PCLPointCloud2> ();
+ pcl::PCLPointCloud2::Ptr temp_cloud =
pcl::make_shared <pcl::PCLPointCloud2> ();
concatenate (*merged_cloud, *selected_points, *temp_cloud);
merged_cloud = temp_cloud;
}
foreach (const CloudComposerItem* input_item, input_data)
{
pcl::PCLPointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <pcl::PCLPointCloud2::ConstPtr> ();
-
- pcl::PCLPointCloud2::Ptr temp_cloud =
boost::make_shared <pcl::PCLPointCloud2> ();
+
+ pcl::PCLPointCloud2::Ptr temp_cloud =
pcl::make_shared <pcl::PCLPointCloud2> ();
concatenate (*merged_cloud, *input_cloud, *temp_cloud);
merged_cloud = temp_cloud;
}
, source_orientation);
output.append (cloud_item);
-
+
return output;
}
#include <pcl/apps/cloud_composer/cloud_view.h>
#include <pcl/apps/cloud_composer/merge_selection.h>
#include <pcl/apps/cloud_composer/transform_clouds.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <QAction>
#include <QFileDialog>
#include <pcl/apps/cloud_composer/tools/euclidean_clustering.h>
#include <pcl/apps/cloud_composer/items/cloud_item.h>
+#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/kdtree.h>
pcl::cloud_composer::EuclideanClusteringTool::EuclideanClusteringTool (PropertiesModel* parameter_model, QObject* parent)
: SplitItemTool (parameter_model, parent)
{
-
+
}
pcl::cloud_composer::EuclideanClusteringTool::~EuclideanClusteringTool ()
{
-
+
}
QList <pcl::cloud_composer::CloudComposerItem*>
qWarning () << "Input vector has more than one item in Euclidean Clustering!";
}
input_item = input_data.value (0);
-
+
if (input_item->type () == CloudComposerItem::CLOUD_ITEM)
{
const CloudItem* cloud_item = dynamic_cast <const CloudItem*> (input_item);
double cluster_tolerance = parameter_model_->getProperty ("Cluster Tolerance").toDouble();
int min_cluster_size = parameter_model_->getProperty ("Min Cluster Size").toInt();
int max_cluster_size = parameter_model_->getProperty ("Max Cluster Size").toInt();
-
+
pcl::PCLPointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <pcl::PCLPointCloud2::ConstPtr> ();
//Get the cloud in template form
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (*input_cloud, *cloud);
-
+
//////////////// THE WORK - COMPUTING CLUSTERS ///////////////////
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
-
+
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (cluster_tolerance);
{
filter.setInputCloud (input_cloud);
// It's annoying that I have to do this, but Euclidean returns a PointIndices struct
- pcl::PointIndices::ConstPtr indices_ptr =
boost::make_shared<pcl::PointIndices>(*it);
+ pcl::PointIndices::ConstPtr indices_ptr =
pcl::make_shared<pcl::PointIndices>(*it);
filter.setIndices (indices_ptr);
extracted_indices->insert (extracted_indices->end (), it->indices.begin (), it->indices.end ());
//This means remove the other points
filter.setKeepOrganized (false);
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2);
filter.filter (*cloud_filtered);
-
+
qDebug() << "Cluster has " << cloud_filtered->width << " data points.";
CloudItem* cloud_item = new CloudItem (input_item->text ()+tr("-Clstr %1").arg(cluster_count)
, cloud_filtered
, source_orientation);
output.append (cloud_item);
++cluster_count;
- }
+ }
//We copy input cloud over for special case that no clusters found, since ExtractIndices doesn't work for 0 length vectors
pcl::PCLPointCloud2::Ptr remainder_cloud (new pcl::PCLPointCloud2(*input_cloud));
if (!cluster_indices.empty ())
{
qCritical () << "Input item in Clustering is not a cloud!!!";
}
-
-
+
+
return output;
}
pcl::cloud_composer::EuclideanClusteringToolFactory::createToolParameterModel (QObject* parent)
{
PropertiesModel* parameter_model = new PropertiesModel(parent);
-
+
parameter_model->addProperty ("Cluster Tolerance", 0.02, Qt::ItemIsEditable | Qt::ItemIsEnabled);
parameter_model->addProperty ("Min Cluster Size", 100, Qt::ItemIsEditable | Qt::ItemIsEnabled);
parameter_model->addProperty ("Max Cluster Size", 25000, Qt::ItemIsEditable | Qt::ItemIsEnabled);
-
return parameter_model;
}
#include <pcl/apps/cloud_composer/tools/sanitize_cloud.h>
#include <pcl/apps/cloud_composer/items/cloud_item.h>
#include <pcl/filters/passthrough.h>
+#include <pcl/memory.h> // for pcl::make_shared
Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
pcl::cloud_composer::SanitizeCloudTool::SanitizeCloudTool (PropertiesModel* parameter_model, QObject* parent)
: ModifyItemTool (parameter_model, parent)
{
-
-
}
pcl::cloud_composer::SanitizeCloudTool::~SanitizeCloudTool ()
{
-
}
QList <pcl::cloud_composer::CloudComposerItem*>
pass_filter.setKeepOrganized (keep_organized);
//Create output cloud
- pcl::PCLPointCloud2::Ptr cloud_filtered =
boost::make_shared<pcl::PCLPointCloud2> ();
+ pcl::PCLPointCloud2::Ptr cloud_filtered =
pcl::make_shared<pcl::PCLPointCloud2> ();
//Filter!
pass_filter.filter (*cloud_filtered);
#include <limits>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <pcl/pcl_macros.h>
#include <pcl/apps/in_hand_scanner/boost.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <pcl/pcl_macros.h>
#include <pcl/common/time.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <pcl/pcl_macros.h>
#include <pcl/common/time.h>
#include <cstdint>
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <pcl/pcl_macros.h>
#include <pcl/apps/in_hand_scanner/eigen.h>
// Convergence and registration failure
float current_fitness = 0.f;
- float previous_fitness = std::numeric_limits <float>::max ();
float delta_fitness = std::numeric_limits <float>::max ();
float overlap = std::numeric_limits <float>::quiet_NaN ();
}
// NOTE: The fitness is calculated with the transformation from the previous iteration (I don't re-calculate it after the transformation estimation). This means that the actual fitness will be one iteration "better" than the calculated fitness suggests. This should be no problem because the difference is small at the state of convergence.
- previous_fitness = current_fitness;
+ float previous_fitness = current_fitness;
current_fitness = squared_distance_sum / static_cast <float> (n_corr);
delta_fitness = std::abs (previous_fitness - current_fitness);
squared_distance_threshold = factor_ * current_fitness;
#pragma once
#include <pcl/common/common.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/console/parse.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/project_inliers.h>
+#include <pcl/filters/voxel_grid.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
+#include <pcl/search/pcl_search.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/filters/project_inliers.h>
#include <pcl/surface/convex_hull.h>
-#include <pcl/segmentation/extract_polygonal_prism_data.h>
-#include <pcl/segmentation/extract_clusters.h>
-
-namespace pcl
-{
- namespace apps
- {
- /** \brief @b DominantPlaneSegmentation performs euclidean segmentation on a scene assuming that a dominant plane exists.
- * \author Aitor Aldoma
- * \ingroup apps
- */
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
- template<typename PointType>
- class PCL_EXPORTS DominantPlaneSegmentation
- {
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
- using KdTreePtr = typename pcl::search::KdTree<PointType>::Ptr;
+namespace pcl {
+namespace apps {
- DominantPlaneSegmentation ()
- {
- min_z_bounds_ = 0;
- max_z_bounds_ = 1.5;
- object_min_height_ = 0.01;
- object_max_height_ = 0.7;
- object_cluster_tolerance_ = 0.05f;
- object_cluster_min_size_ = 500;
- k_ = 50;
- sac_distance_threshold_ = 0.01;
- downsample_leaf_ = 0.005f;
- wsize_ = 5;
- }
+/**
+ * \brief @b DominantPlaneSegmentation performs euclidean segmentation on a scene
+ * assuming that a dominant plane exists.
+ * \author Aitor Aldoma
+ * \ingroup apps
+ */
+template <typename PointType>
+class PCL_EXPORTS DominantPlaneSegmentation {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+ using KdTreePtr = typename pcl::search::KdTree<PointType>::Ptr;
- /* \brief Extract the clusters.
- * \param clusters Clusters extracted from the initial point cloud at the resolution size
- * specified by downsample_leaf_
- */
- void
- compute (std::vector<CloudPtr> & clusters);
+ DominantPlaneSegmentation()
+ {
+ min_z_bounds_ = 0;
+ max_z_bounds_ = 1.5;
+ object_min_height_ = 0.01;
+ object_max_height_ = 0.7;
+ object_cluster_tolerance_ = 0.05f;
+ object_cluster_min_size_ = 500;
+ k_ = 50;
+ sac_distance_threshold_ = 0.01;
+ downsample_leaf_ = 0.005f;
+ wsize_ = 5;
+ }
- /* \brief Extract the clusters.
- * \param clusters Clusters extracted from the initial point cloud. The returned
- * clusters are not downsampled.
- */
- void
- compute_full (std::vector<CloudPtr> & clusters);
+ /**
+ * \brief Extract the clusters.
+ * \param clusters Clusters extracted from the initial point cloud at the resolution
+ * size specified by downsample_leaf_
+ */
+ void
+ compute(std::vector<CloudPtr>& clusters);
- /* \brief Extract clusters on a plane using connected components on an organized pointcloud.
- * The method expects a the input cloud to have the is_dense attribute set to false.
- * \param clusters Clusters extracted from the initial point cloud. The returned
- * clusters are not downsampled.
- */
- void
- compute_fast (std::vector<CloudPtr> & clusters);
+ /**
+ * \brief Extract the clusters.
+ * \param clusters Clusters extracted from the initial point cloud. The returned
+ * clusters are not downsampled.
+ */
+ void
+ compute_full(std::vector<CloudPtr>& clusters);
- /* \brief Computes the table plane.
- */
- void
- compute_table_plane();
+ /**
+ * \brief Extract clusters on a plane using connected components on an organized
+ * pointcloud. The method expects a the input cloud to have the is_dense attribute set
+ * to false.
+ * \param clusters Clusters extracted from the initial point cloud. The returned
+ * clusters are not downsampled.
+ */
+ void
+ compute_fast(std::vector<CloudPtr>& clusters);
- /* \brief Sets the input point cloud.
- * \param cloud_in The input point cloud.
- */
- void
- setInputCloud (CloudPtr & cloud_in)
- {
- input_ = cloud_in;
- }
+ /**
+ * \brief Computes the table plane.
+ */
+ void
+ compute_table_plane();
- /* \brief Returns the table coefficients after computation
- * \param model represents the normal and the position of the plane (a,b,c,d)
- */
- void
- getTableCoefficients (Eigen::Vector4f & model)
- {
- model = table_coeffs_;
- }
+ /**
+ * \brief Sets the input point cloud.
+ * \param cloud_in The input point cloud.
+ */
+ void
+ setInputCloud(CloudPtr& cloud_in)
+ {
+ input_ = cloud_in;
+ }
- /* \brief Sets minimum distance between clusters
- * \param d distance (in meters)
- */
- void
- setDistanceBetweenClusters (float d)
- {
- object_cluster_tolerance_ = d;
- }
+ /**
+ * \brief Returns the table coefficients after computation
+ * \param model represents the normal and the position of the plane (a,b,c,d)
+ */
+ void
+ getTableCoefficients(Eigen::Vector4f& model)
+ {
+ model = table_coeffs_;
+ }
- /* \brief Sets minimum size of the clusters.
- * \param size number of points
- */
- void
- setMinClusterSize (int size)
- {
- object_cluster_min_size_ = size;
- }
+ /**
+ * \brief Sets minimum distance between clusters
+ * \param d distance (in meters)
+ */
+ void
+ setDistanceBetweenClusters(float d)
+ {
+ object_cluster_tolerance_ = d;
+ }
- /* \brief Sets the min height of the clusters in order to be considered.
- * \param h minimum height (in meters)
- */
- void
- setObjectMinHeight (double h)
- {
- object_min_height_ = h;
- }
+ /**
+ * \brief Sets minimum size of the clusters.
+ * \param size number of points
+ */
+ void
+ setMinClusterSize(int size)
+ {
+ object_cluster_min_size_ = size;
+ }
- /* \brief Sets the max height of the clusters in order to be considered.
- * \param h max height (in meters)
- */
- void
- setObjectMaxHeight (double h)
- {
- object_max_height_ = h;
- }
+ /**
+ * \brief Sets the min height of the clusters in order to be considered.
+ * \param h minimum height (in meters)
+ */
+ void
+ setObjectMinHeight(double h)
+ {
+ object_min_height_ = h;
+ }
- /* \brief Sets minimum distance from the camera for a point to be considered.
- * \param z distance (in meters)
- */
- void
- setMinZBounds (double z)
- {
- min_z_bounds_ = z;
- }
- /* \brief Sets maximum distance from the camera for a point to be considered.
- * \param z distance (in meters)
- */
- void
- setMaxZBounds (double z)
- {
- max_z_bounds_ = z;
- }
+ /**
+ * \brief Sets the max height of the clusters in order to be considered.
+ * \param h max height (in meters)
+ */
+ void
+ setObjectMaxHeight(double h)
+ {
+ object_max_height_ = h;
+ }
- /* \brief Sets the number of neighbors used for normal estimation.
- * \param k number of neighbors
- */
- void setKNeighbors(int k) {
- k_ = k;
- }
+ /**
+ * \brief Sets minimum distance from the camera for a point to be considered.
+ * \param z distance (in meters)
+ */
+ void
+ setMinZBounds(double z)
+ {
+ min_z_bounds_ = z;
+ }
+ /**
+ * \brief Sets maximum distance from the camera for a point to be considered.
+ * \param z distance (in meters)
+ */
+ void
+ setMaxZBounds(double z)
+ {
+ max_z_bounds_ = z;
+ }
- /* \brief Set threshold for SAC plane segmentation
- * \param d threshold (in meters)
- */
- void setSACThreshold(double d) {
- sac_distance_threshold_ = d;
- }
+ /**
+ * \brief Sets the number of neighbors used for normal estimation.
+ * \param k number of neighbors
+ */
+ void
+ setKNeighbors(int k)
+ {
+ k_ = k;
+ }
- /* \brief Set downsampling resolution.
- * \param d resolution (in meters)
- */
- void
- setDownsamplingSize (float d)
- {
- downsample_leaf_ = d;
- }
+ /**
+ * \brief Set threshold for SAC plane segmentation
+ * \param d threshold (in meters)
+ */
+ void
+ setSACThreshold(double d)
+ {
+ sac_distance_threshold_ = d;
+ }
- /* \brief Set window size in pixels for CC used in compute_fast method
- * \param w window size (in pixels)
- */
- void setWSize(int w) {
- wsize_ = w;
- }
+ /**
+ * \brief Set downsampling resolution.
+ * \param d resolution (in meters)
+ */
+ void
+ setDownsamplingSize(float d)
+ {
+ downsample_leaf_ = d;
+ }
- /* \brief Returns the indices of the clusters found by the segmentation
- * NOTE: This function returns only valid indices if the compute_fast method is used
- * \param indices indices of the clusters
- */
- void getIndicesClusters(std::vector<pcl::PointIndices> & indices) {
- indices = indices_clusters_;
- }
+ /**
+ * \brief Set window size in pixels for CC used in compute_fast method
+ * \param w window size (in pixels)
+ */
+ void
+ setWSize(int w)
+ {
+ wsize_ = w;
+ }
- private:
+ /**
+ * \brief Returns the indices of the clusters found by the segmentation
+ * NOTE: This function returns only valid indices if the compute_fast method is used
+ * \param indices indices of the clusters
+ */
+ void
+ getIndicesClusters(std::vector<pcl::PointIndices>& indices)
+ {
+ indices = indices_clusters_;
+ }
- int
- check (pcl::PointXYZI & p1, pcl::PointXYZI & p2, float, float max_dist)
- {
- if (p1.intensity == 0) //new label
- return 1;
- //compute distance and check against max_dist
- if ((p1.getVector3fMap () - p2.getVector3fMap ()).norm () <= max_dist)
- {
- p2.intensity = p1.intensity;
- return 0;
- }
- //new label
- return 1;
- }
+private:
+ int
+ check(pcl::PointXYZI& p1, pcl::PointXYZI& p2, float, float max_dist)
+ {
+ if (p1.intensity == 0) // new label
+ return 1;
+ // compute distance and check against max_dist
+ if ((p1.getVector3fMap() - p2.getVector3fMap()).norm() <= max_dist) {
+ p2.intensity = p1.intensity;
+ return 0;
+ }
+ // new label
+ return 1;
+ }
- //components needed for cluster segmentation and plane extraction
-
pcl::PassThrough<PointType> pass_;
-
pcl::VoxelGrid<PointType> grid_;
-
pcl::NormalEstimation<PointType, pcl::Normal> n3d_;
-
pcl::SACSegmentationFromNormals<PointType, pcl::Normal> seg_;
-
pcl::ProjectInliers<PointType> proj_;
-
pcl::ProjectInliers<PointType> bb_cluster_proj_;
-
pcl::ConvexHull<PointType> hull_;
-
pcl::ExtractPolygonalPrismData<PointType> prism_;
-
pcl::EuclideanClusterExtraction<PointType> cluster_;
+ // components needed for cluster segmentation and plane extraction
+ pcl::PassThrough<PointType> pass_;
+ pcl::VoxelGrid<PointType> grid_;
+ pcl::NormalEstimation<PointType, pcl::Normal> n3d_;
+ pcl::SACSegmentationFromNormals<PointType, pcl::Normal> seg_;
+ pcl::ProjectInliers<PointType> proj_;
+ pcl::ProjectInliers<PointType> bb_cluster_proj_;
+ pcl::ConvexHull<PointType> hull_;
+ pcl::ExtractPolygonalPrismData<PointType> prism_;
+ pcl::EuclideanClusterExtraction<PointType> cluster_;
- /** \brief Input cloud from which to extract clusters */
- CloudPtr input_;
- /** \brief Table coefficients (a,b,c,d) */
- Eigen::Vector4f table_coeffs_;
- /** \brief Downsampling resolution. */
- float downsample_leaf_;
- /** \brief Number of neighbors for normal estimation */
- int k_;
- /** \brief Keep points farther away than min_z_bounds */
- double min_z_bounds_;
- /** \brief Keep points closer than max_z_bounds */
- double max_z_bounds_;
- /** \brief Threshold for SAC plane segmentation */
- double sac_distance_threshold_;
- /** \brief Min height from the table plane object points will be considered from */
- double object_min_height_;
- /** \brief Max height from the table plane */
- double object_max_height_;
- /** \brief Tolerance between different clusters */
- float object_cluster_tolerance_;
- /** \brief Minimum size for a cluster, clusters smaller than this won't be returned */
- int object_cluster_min_size_;
- /** \brief Window size in pixels for CC in compute_fast method */
- int wsize_;
- /** \brief Indices of the clusters to the main cloud found by the segmentation */
- std::vector<pcl::PointIndices> indices_clusters_;
+ /** \brief Input cloud from which to extract clusters */
+ CloudPtr input_;
+ /** \brief Table coefficients (a,b,c,d) */
+ Eigen::Vector4f table_coeffs_;
+ /** \brief Downsampling resolution. */
+ float downsample_leaf_;
+ /** \brief Number of neighbors for normal estimation */
+ int k_;
+ /** \brief Keep points farther away than min_z_bounds */
+ double min_z_bounds_;
+ /** \brief Keep points closer than max_z_bounds */
+ double max_z_bounds_;
+ /** \brief Threshold for SAC plane segmentation */
+ double sac_distance_threshold_;
+ /** \brief Min height from the table plane object points will be considered from */
+ double object_min_height_;
+ /** \brief Max height from the table plane */
+ double object_max_height_;
+ /** \brief Tolerance between different clusters */
+ float object_cluster_tolerance_;
+ /** \brief Minimum size for a cluster, clusters smaller than this won't be returned */
+ int object_cluster_min_size_;
+ /** \brief Window size in pixels for CC in compute_fast method */
+ int wsize_;
+ /** \brief Indices of the clusters to the main cloud found by the segmentation */
+ std::vector<pcl::PointIndices> indices_clusters_;
+};
- };
- }
-}
+} // namespace apps
+} // namespace pcl
#ifdef PCL_NO_PRECOMPILE
#include <pcl/apps/impl/dominant_plane_segmentation.hpp>
#pragma once
-namespace face_detection_apps_utils
-{
- inline bool readMatrixFromFile(const std::string& file, Eigen::Matrix4f & matrix)
- {
+namespace face_detection_apps_utils {
- std::ifstream in;
- in.open (file.c_str (), std::ifstream::in);
- if (!in.is_open ())
- {
- return false;
- }
+inline bool
+readMatrixFromFile(const std::string& file, Eigen::Matrix4f& matrix)
+{
- char linebuf[1024];
- in.getline (linebuf, 1024);
- std::string line (linebuf);
- std::vector < std::string > strs_2;
- boost::split (strs_2, line, boost::is_any_of (" "));
+ std::ifstream in;
+ in.open(file.c_str(), std::ifstream::in);
+ if (!in.is_open()) {
+ return false;
+ }
- for (int i = 0; i < 16; i++)
- {
- matrix (i / 4, i % 4) = static_cast<float> (atof (strs_2[i].c_str ()));
- }
+ char linebuf[1024];
+ in.getline(linebuf, 1024);
+ std::string line(linebuf);
+ std::vector<std::string> strs_2;
+ boost::split(strs_2, line, boost::is_any_of(" "));
- return true;
+ for (int i = 0; i < 16; i++) {
+ matrix(i / 4, i % 4) = static_cast<float>(atof(strs_2[i].c_str()));
}
- inline bool sortFiles(const std::string & file1, const std::string & file2)
- {
- std::vector < std::string > strs1;
- boost::split (strs1, file1, boost::is_any_of ("/"));
+ return true;
+}
- std::vector < std::string > strs2;
- boost::split (strs2, file2, boost::is_any_of ("/"));
+inline bool
+sortFiles(const std::string& file1, const std::string& file2)
+{
+ std::vector<std::string> strs1;
+ boost::split(strs1, file1, boost::is_any_of("/"));
- std::string id_1 = strs1[strs1.size () - 1];
- std::string id_2 = strs2[strs2.size () - 1];
+ std::vector<std::string> strs2;
+ boost::split(strs2, file2, boost::is_any_of("/"));
- {
- std::vector < std::string > strs1;
- boost::split (strs1, id_1, boost::is_any_of ("_"));
+ std::string id_1 = strs1[strs1.size() - 1];
+ std::string id_2 = strs2[strs2.size() - 1];
- std::vector < std::string > strs2;
- boost::split (strs2, id_2, boost::is_any_of ("_"));
+ {
+ std::vector<std::string> strs1;
+ boost::split(strs1, id_1, boost::is_any_of("_"));
- std::string id_1 = strs1[strs1.size () - 1];
- std::string id_2 = strs2[strs2.size () - 1];
+ std::vector<std::string> strs2;
+ boost::split(strs2, id_2, boost::is_any_of("_"));
- std::size_t pos1 = id_1.find (".pcd");
- std::size_t pos2 = id_2.find (".pcd");
+ std::string id_1 = strs1[strs1.size() - 1];
+ std::string id_2 = strs2[strs2.size() - 1];
- id_1 = id_1.substr (0, pos1);
- id_2 = id_2.substr (0, pos2);
+ std::size_t pos1 = id_1.find(".pcd");
+ std::size_t pos2 = id_2.find(".pcd");
- return atoi (id_1.c_str ()) < atoi (id_2.c_str ());
- }
+ id_1 = id_1.substr(0, pos1);
+ id_2 = id_2.substr(0, pos2);
+
+ return atoi(id_1.c_str()) < atoi(id_2.c_str());
}
+}
- inline
- void getFilesInDirectory(bf::path & dir, std::string & rel_path_so_far, std::vector<std::string> & relative_paths, std::string & ext)
- {
- for (const auto& dir_entry : bf::directory_iterator(dir))
- {
- //check if its a directory, then get models in it
- if (bf::is_directory (dir_entry))
- {
- std::string so_far = rel_path_so_far + (dir_entry.path().filename()).string() + "/";
-
- bf::path curr_path = dir_entry.path ();
- getFilesInDirectory (curr_path, so_far, relative_paths, ext);
- } else
- {
- //check that it is a ply file and then add, otherwise ignore..
- std::vector < std::string > strs;
- std::string file = (dir_entry.path().filename()).string();
- boost::split (strs, file, boost::is_any_of ("."));
- std::string extension = strs[strs.size () - 1];
-
- if (extension == ext)
- {
- std::string path = rel_path_so_far + (dir_entry.path().filename()).string();
- relative_paths.push_back (path);
- }
+inline void
+getFilesInDirectory(bf::path& dir,
+ std::string& rel_path_so_far,
+ std::vector<std::string>& relative_paths,
+ std::string& ext)
+{
+ for (const auto& dir_entry : bf::directory_iterator(dir)) {
+ // check if its a directory, then get models in it
+ if (bf::is_directory(dir_entry)) {
+ std::string so_far =
+ rel_path_so_far + (dir_entry.path().filename()).string() + "/";
+
+ bf::path curr_path = dir_entry.path();
+ getFilesInDirectory(curr_path, so_far, relative_paths, ext);
+ }
+ else {
+ // check that it is a ply file and then add, otherwise ignore..
+ std::vector<std::string> strs;
+ std::string file = (dir_entry.path().filename()).string();
+ boost::split(strs, file, boost::is_any_of("."));
+ std::string extension = strs[strs.size() - 1];
+
+ if (extension == ext) {
+ std::string path = rel_path_so_far + (dir_entry.path().filename()).string();
+ relative_paths.push_back(path);
}
}
}
+}
- void displayHeads(std::vector<Eigen::VectorXf> & heads, pcl::visualization::PCLVisualizer & vis)
- {
- for (std::size_t i = 0; i < heads.size (); i++)
- {
- std::stringstream name;
- name << "sphere" << i;
- pcl::PointXYZ center_point;
- center_point.x = heads[i][0];
- center_point.y = heads[i][1];
- center_point.z = heads[i][2];
- vis.addSphere (center_point, 0.02, 0, 255, 0, name.str ());
-
- pcl::ModelCoefficients cylinder_coeff;
- cylinder_coeff.values.resize (7); // We need 7 values
- cylinder_coeff.values[0] = center_point.x;
- cylinder_coeff.values[1] = center_point.y;
- cylinder_coeff.values[2] = center_point.z;
-
- Eigen::Vector3f vec = Eigen::Vector3f::UnitZ () * -1.f;
- Eigen::Matrix3f matrixxx;
-
- matrixxx = Eigen::AngleAxisf (heads[i][3], Eigen::Vector3f::UnitX ()) * Eigen::AngleAxisf (heads[i][4], Eigen::Vector3f::UnitY ())
- * Eigen::AngleAxisf (heads[i][5], Eigen::Vector3f::UnitZ ());
-
- vec = matrixxx * vec;
-
- cylinder_coeff.values[3] = vec[0];
- cylinder_coeff.values[4] = vec[1];
- cylinder_coeff.values[5] = vec[2];
-
- cylinder_coeff.values[6] = 0.01f;
- name << "cylinder";
- vis.addCylinder (cylinder_coeff, name.str ());
- }
+void
+displayHeads(std::vector<Eigen::VectorXf>& heads,
+ pcl::visualization::PCLVisualizer& vis)
+{
+ for (std::size_t i = 0; i < heads.size(); i++) {
+ std::stringstream name;
+ name << "sphere" << i;
+ pcl::PointXYZ center_point;
+ center_point.x = heads[i][0];
+ center_point.y = heads[i][1];
+ center_point.z = heads[i][2];
+ vis.addSphere(center_point, 0.02, 0, 255, 0, name.str());
+
+ pcl::ModelCoefficients cylinder_coeff;
+ cylinder_coeff.values.resize(7); // We need 7 values
+ cylinder_coeff.values[0] = center_point.x;
+ cylinder_coeff.values[1] = center_point.y;
+ cylinder_coeff.values[2] = center_point.z;
+
+ Eigen::Vector3f vec = Eigen::Vector3f::UnitZ() * -1.f;
+ Eigen::Matrix3f matrixxx;
+
+ matrixxx = Eigen::AngleAxisf(heads[i][3], Eigen::Vector3f::UnitX()) *
+ Eigen::AngleAxisf(heads[i][4], Eigen::Vector3f::UnitY()) *
+ Eigen::AngleAxisf(heads[i][5], Eigen::Vector3f::UnitZ());
+
+ vec = matrixxx * vec;
+
+ cylinder_coeff.values[3] = vec[0];
+ cylinder_coeff.values[4] = vec[1];
+ cylinder_coeff.values[5] = vec[2];
+
+ cylinder_coeff.values[6] = 0.01f;
+ name << "cylinder";
+ vis.addCylinder(cylinder_coeff, name.str());
}
}
+
+} // namespace face_detection_apps_utils
#include <mutex>
-namespace OpenNIFrameSource
-{
-
- using PointT = pcl::PointXYZRGBA;
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudPtr = pcl::PointCloud<PointT>::Ptr;
- using PointCloudConstPtr = pcl::PointCloud<PointT>::ConstPtr;
-
- /* A simple class for capturing data from an OpenNI camera */
- class PCL_EXPORTS OpenNIFrameSource
- {
- public:
- OpenNIFrameSource (const std::string& device_id = "");
- ~OpenNIFrameSource ();
-
- const PointCloudPtr
- snap ();
- bool
- isActive ();
- void
- onKeyboardEvent (const pcl::visualization::KeyboardEvent & event);
-
- protected:
- void
- onNewFrame (const PointCloudConstPtr &cloud);
-
- pcl::OpenNIGrabber grabber_;
- PointCloudPtr most_recent_frame_;
- int frame_counter_;
- std::mutex mutex_;
- bool active_;
- };
-
-}
+namespace OpenNIFrameSource {
+
+using PointT = pcl::PointXYZRGBA;
+using PointCloud = pcl::PointCloud<PointT>;
+using PointCloudPtr = pcl::PointCloud<PointT>::Ptr;
+using PointCloudConstPtr = pcl::PointCloud<PointT>::ConstPtr;
+
+/* A simple class for capturing data from an OpenNI camera */
+class PCL_EXPORTS OpenNIFrameSource {
+public:
+ OpenNIFrameSource(const std::string& device_id = "");
+ ~OpenNIFrameSource();
+
+ const PointCloudPtr
+ snap();
+ bool
+ isActive() const;
+ void
+ onKeyboardEvent(const pcl::visualization::KeyboardEvent& event);
+
+protected:
+ void
+ onNewFrame(const PointCloudConstPtr& cloud);
+
+ pcl::OpenNIGrabber grabber_;
+ PointCloudPtr most_recent_frame_;
+ int frame_counter_;
+ std::mutex mutex_;
+ bool active_;
+};
+
+} // namespace OpenNIFrameSource
*
*/
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/features/integral_image_normal.h>
#include <pcl/common/time.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/memory.h> // for pcl::make_shared
-template<typename PointType> void
-pcl::apps::DominantPlaneSegmentation<PointType>::compute_table_plane ()
+template <typename PointType>
+void
+pcl::apps::DominantPlaneSegmentation<PointType>::compute_table_plane()
{
// Has the input dataset been set already?
- if (!input_)
- {
- PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
+ if (!input_) {
+ PCL_WARN("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
CloudConstPtr cloud_;
- CloudPtr cloud_filtered_ (new Cloud ());
- CloudPtr cloud_downsampled_ (new Cloud ());
- pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
- pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
- pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
- CloudPtr table_projected_ (new Cloud ());
- CloudPtr table_hull_ (new Cloud ());
+ CloudPtr cloud_filtered_(new Cloud());
+ CloudPtr cloud_downsampled_(new Cloud());
+ pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_(new pcl::PointCloud<pcl::Normal>());
+ pcl::PointIndices::Ptr table_inliers_(new pcl::PointIndices());
+ pcl::ModelCoefficients::Ptr table_coefficients_(new pcl::ModelCoefficients());
+ CloudPtr table_projected_(new Cloud());
+ CloudPtr table_hull_(new Cloud());
- typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
+ typename pcl::search::KdTree<PointType>::Ptr normals_tree_(
+ new pcl::search::KdTree<PointType>);
// Normal estimation parameters
- n3d_.setKSearch (k_);
- n3d_.setSearchMethod (normals_tree_);
+ n3d_.setKSearch(k_);
+ n3d_.setSearchMethod(normals_tree_);
// Table model fitting parameters
- seg_.setDistanceThreshold (sac_distance_threshold_);
- seg_.setMaxIterations (2000);
- seg_.setNormalDistanceWeight (0.1);
- seg_.setOptimizeCoefficients (true);
- seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
- seg_.setMethodType (pcl::SAC_RANSAC);
- seg_.setProbability (0.99);
+ seg_.setDistanceThreshold(sac_distance_threshold_);
+ seg_.setMaxIterations(2000);
+ seg_.setNormalDistanceWeight(0.1);
+ seg_.setOptimizeCoefficients(true);
+ seg_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
+ seg_.setMethodType(pcl::SAC_RANSAC);
+ seg_.setProbability(0.99);
- proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
- bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
+ proj_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
+ bb_cluster_proj_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
// ---[ PassThroughFilter
- pass_.setFilterLimits (float (min_z_bounds_), float (max_z_bounds_));
- pass_.setFilterFieldName ("z");
- pass_.setInputCloud (input_);
- pass_.filter (*cloud_filtered_);
-
- if (int (cloud_filtered_->points.size ()) < k_)
- {
- PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
- cloud_filtered_->points.size ());
+ pass_.setFilterLimits(float(min_z_bounds_), float(max_z_bounds_));
+ pass_.setFilterFieldName("z");
+ pass_.setInputCloud(input_);
+ pass_.filter(*cloud_filtered_);
+
+ if (int(cloud_filtered_->points.size()) < k_) {
+ PCL_WARN("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
+ cloud_filtered_->points.size());
return;
}
// Downsample the point cloud
- grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
- grid_.setDownsampleAllData (false);
- grid_.setInputCloud (cloud_filtered_);
- grid_.filter (*cloud_downsampled_);
+ grid_.setLeafSize(downsample_leaf_, downsample_leaf_, downsample_leaf_);
+ grid_.setDownsampleAllData(false);
+ grid_.setInputCloud(cloud_filtered_);
+ grid_.filter(*cloud_downsampled_);
// ---[ Estimate the point normals
- n3d_.setInputCloud (cloud_downsampled_);
- n3d_.compute (*cloud_normals_);
+ n3d_.setInputCloud(cloud_downsampled_);
+ n3d_.compute(*cloud_normals_);
// ---[ Perform segmentation
- seg_.setInputCloud (cloud_downsampled_);
- seg_.setInputNormals (cloud_normals_);
- seg_.segment (*table_inliers_, *table_coefficients_);
+ seg_.setInputCloud(cloud_downsampled_);
+ seg_.setInputNormals(cloud_normals_);
+ seg_.segment(*table_inliers_, *table_coefficients_);
- if (table_inliers_->indices.empty ())
- {
- PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
+ if (table_inliers_->indices.empty()) {
+ PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
- proj_.setInputCloud (cloud_downsampled_);
- proj_.setIndices (table_inliers_);
- proj_.setModelCoefficients (table_coefficients_);
- proj_.filter (*table_projected_);
+ proj_.setInputCloud(cloud_downsampled_);
+ proj_.setIndices(table_inliers_);
+ proj_.setModelCoefficients(table_coefficients_);
+ proj_.filter(*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
- CloudPtr table_hull (new Cloud ());
- hull_.setInputCloud (table_projected_);
- hull_.reconstruct (*table_hull, polygons);
+ CloudPtr table_hull(new Cloud());
+ hull_.setInputCloud(table_projected_);
+ hull_.reconstruct(*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
- Eigen::Vector4f vp (0, 0, 0, 0);
+ Eigen::Vector4f vp(0, 0, 0, 0);
// See if we need to flip any plane normals
- vp -= table_hull->points[0].getVector4fMap ();
+ vp -= table_hull->points[0].getVector4fMap();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
- float cos_theta = vp.dot (model_coefficients);
+ float cos_theta = vp.dot(model_coefficients);
// Flip the plane normal
- if (cos_theta < 0)
- {
+ if (cos_theta < 0) {
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
- model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
+ model_coefficients[3] =
+ -1 * (model_coefficients.dot(table_hull->points[0].getVector4fMap()));
}
- //Set table_coeffs
+ // Set table_coeffs
table_coeffs_ = model_coefficients;
}
-template<typename PointType> void
-pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast (std::vector<CloudPtr> & clusters)
+template <typename PointType>
+void
+pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast(
+ std::vector<CloudPtr>& clusters)
{
// Has the input dataset been set already?
- if (!input_)
- {
- PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
+ if (!input_) {
+ PCL_WARN("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
// Is the input dataset organized?
- if (input_->is_dense)
- {
- PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] compute_fast can only be used with organized point clouds!\n");
+ if (input_->is_dense) {
+ PCL_WARN("[pcl::apps::DominantPlaneSegmentation] compute_fast can only be used "
+ "with organized point clouds!\n");
return;
}
CloudConstPtr cloud_;
- CloudPtr cloud_filtered_ (new Cloud ());
- CloudPtr cloud_downsampled_ (new Cloud ());
- pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
- pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
- pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
- CloudPtr table_projected_ (new Cloud ());
- CloudPtr table_hull_ (new Cloud ());
- CloudPtr cloud_objects_ (new Cloud ());
- CloudPtr cluster_object_ (new Cloud ());
-
- typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
- typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
- clusters_tree_->setEpsilon (1);
+ CloudPtr cloud_filtered_(new Cloud());
+ CloudPtr cloud_downsampled_(new Cloud());
+ pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_(new pcl::PointCloud<pcl::Normal>());
+ pcl::PointIndices::Ptr table_inliers_(new pcl::PointIndices());
+ pcl::ModelCoefficients::Ptr table_coefficients_(new pcl::ModelCoefficients());
+ CloudPtr table_projected_(new Cloud());
+ CloudPtr table_hull_(new Cloud());
+ CloudPtr cloud_objects_(new Cloud());
+ CloudPtr cluster_object_(new Cloud());
+
+ typename pcl::search::KdTree<PointType>::Ptr normals_tree_(
+ new pcl::search::KdTree<PointType>);
+ typename pcl::search::KdTree<PointType>::Ptr clusters_tree_(
+ new pcl::search::KdTree<PointType>);
+ clusters_tree_->setEpsilon(1);
// Normal estimation parameters
- n3d_.setKSearch (k_);
- n3d_.setSearchMethod (normals_tree_);
+ n3d_.setKSearch(k_);
+ n3d_.setSearchMethod(normals_tree_);
// Table model fitting parameters
- seg_.setDistanceThreshold (sac_distance_threshold_);
- seg_.setMaxIterations (2000);
- seg_.setNormalDistanceWeight (0.1);
- seg_.setOptimizeCoefficients (true);
- seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
- seg_.setMethodType (pcl::SAC_RANSAC);
- seg_.setProbability (0.99);
+ seg_.setDistanceThreshold(sac_distance_threshold_);
+ seg_.setMaxIterations(2000);
+ seg_.setNormalDistanceWeight(0.1);
+ seg_.setOptimizeCoefficients(true);
+ seg_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
+ seg_.setMethodType(pcl::SAC_RANSAC);
+ seg_.setProbability(0.99);
- proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
- bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
+ proj_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
+ bb_cluster_proj_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
- prism_.setHeightLimits (object_min_height_, object_max_height_);
+ prism_.setHeightLimits(object_min_height_, object_max_height_);
// Clustering parameters
- cluster_.setClusterTolerance (object_cluster_tolerance_);
- cluster_.setMinClusterSize (object_cluster_min_size_);
- cluster_.setSearchMethod (clusters_tree_);
+ cluster_.setClusterTolerance(object_cluster_tolerance_);
+ cluster_.setMinClusterSize(object_cluster_min_size_);
+ cluster_.setSearchMethod(clusters_tree_);
// ---[ PassThroughFilter
- pass_.setFilterLimits (float (min_z_bounds_), float (max_z_bounds_));
- pass_.setFilterFieldName ("z");
- pass_.setInputCloud (input_);
- pass_.filter (*cloud_filtered_);
-
- if (int (cloud_filtered_->points.size ()) < k_)
- {
- PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
- cloud_filtered_->points.size ());
+ pass_.setFilterLimits(float(min_z_bounds_), float(max_z_bounds_));
+ pass_.setFilterFieldName("z");
+ pass_.setInputCloud(input_);
+ pass_.filter(*cloud_filtered_);
+
+ if (int(cloud_filtered_->points.size()) < k_) {
+ PCL_WARN("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
+ cloud_filtered_->points.size());
return;
}
// Downsample the point cloud
- grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
- grid_.setDownsampleAllData (false);
- grid_.setInputCloud (cloud_filtered_);
- grid_.filter (*cloud_downsampled_);
+ grid_.setLeafSize(downsample_leaf_, downsample_leaf_, downsample_leaf_);
+ grid_.setDownsampleAllData(false);
+ grid_.setInputCloud(cloud_filtered_);
+ grid_.filter(*cloud_downsampled_);
// ---[ Estimate the point normals
- n3d_.setInputCloud (cloud_downsampled_);
- n3d_.compute (*cloud_normals_);
+ n3d_.setInputCloud(cloud_downsampled_);
+ n3d_.compute(*cloud_normals_);
// ---[ Perform segmentation
- seg_.setInputCloud (cloud_downsampled_);
- seg_.setInputNormals (cloud_normals_);
- seg_.segment (*table_inliers_, *table_coefficients_);
+ seg_.setInputCloud(cloud_downsampled_);
+ seg_.setInputNormals(cloud_normals_);
+ seg_.segment(*table_inliers_, *table_coefficients_);
- if (table_inliers_->indices.empty ())
- {
- PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
+ if (table_inliers_->indices.empty()) {
+ PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
- proj_.setInputCloud (cloud_downsampled_);
- proj_.setIndices (table_inliers_);
- proj_.setModelCoefficients (table_coefficients_);
- proj_.filter (*table_projected_);
+ proj_.setInputCloud(cloud_downsampled_);
+ proj_.setIndices(table_inliers_);
+ proj_.setModelCoefficients(table_coefficients_);
+ proj_.filter(*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
- CloudPtr table_hull (new Cloud ());
- hull_.setInputCloud (table_projected_);
- hull_.reconstruct (*table_hull, polygons);
+ CloudPtr table_hull(new Cloud());
+ hull_.setInputCloud(table_projected_);
+ hull_.reconstruct(*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
- Eigen::Vector4f vp (0, 0, 0, 0);
+ Eigen::Vector4f vp(0, 0, 0, 0);
// See if we need to flip any plane normals
- vp -= table_hull->points[0].getVector4fMap ();
+ vp -= table_hull->points[0].getVector4fMap();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
- float cos_theta = vp.dot (model_coefficients);
+ float cos_theta = vp.dot(model_coefficients);
// Flip the plane normal
- if (cos_theta < 0)
- {
+ if (cos_theta < 0) {
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
- model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
+ model_coefficients[3] =
+ -1 * (model_coefficients.dot(table_hull->points[0].getVector4fMap()));
}
- //Set table_coeffs
+ // Set table_coeffs
table_coeffs_ = model_coefficients;
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices;
- prism_.setInputCloud (input_);
- prism_.setInputPlanarHull (table_hull);
- prism_.segment (cloud_object_indices);
+ prism_.setInputCloud(input_);
+ prism_.setInputPlanarHull(table_hull);
+ prism_.segment(cloud_object_indices);
pcl::ExtractIndices<PointType> extract_object_indices;
- extract_object_indices.setInputCloud (input_);
- extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
- extract_object_indices.filter (*cloud_objects_);
+ extract_object_indices.setInputCloud(input_);
+ extract_object_indices.setIndices(
+ pcl::make_shared<const pcl::PointIndices>(cloud_object_indices));
+ extract_object_indices.filter(*cloud_objects_);
- //create new binary pointcloud with intensity values (0 and 1), 0 for non-object pixels and 1 otherwise
- pcl::PointCloud<pcl::PointXYZI>::Ptr binary_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ // create new binary pointcloud with intensity values (0 and 1),
+ // 0 for non-object pixels and 1 otherwise
+ pcl::PointCloud<pcl::PointXYZI>::Ptr binary_cloud(
+ new pcl::PointCloud<pcl::PointXYZI>);
{
binary_cloud->width = input_->width;
binary_cloud->height = input_->height;
- binary_cloud->points.resize (input_->points.size ());
+ binary_cloud->points.resize(input_->points.size());
binary_cloud->is_dense = input_->is_dense;
- for (const int &idx : cloud_object_indices.indices)
- {
- binary_cloud->points[idx].getVector4fMap () = input_->points[idx].getVector4fMap ();
+ for (const int& idx : cloud_object_indices.indices) {
+ binary_cloud->points[idx].getVector4fMap() = input_->points[idx].getVector4fMap();
binary_cloud->points[idx].intensity = 1.0;
}
}
- //connected components on the binary image
+ // connected components on the binary image
std::map<float, float> connected_labels;
float c_intensity = 0.1f;
{
int wsize = wsize_;
- for (int i = 0; i < int (binary_cloud->width); i++)
- {
- for (int j = 0; j < int (binary_cloud->height); j++)
- {
- if (binary_cloud->at (i, j).intensity != 0)
- {
- //check neighboring pixels, first left and then top
- //be aware of margins
-
- if ((i - 1) < 0 && (j - 1) < 0)
- {
- //top-left pixel
- (*binary_cloud) (i, j).intensity = c_intensity;
+ for (int i = 0; i < int(binary_cloud->width); i++) {
+ for (int j = 0; j < int(binary_cloud->height); j++) {
+ if (binary_cloud->at(i, j).intensity != 0) {
+ // check neighboring pixels, first left and then top
+ // be aware of margins
+
+ if ((i - 1) < 0 && (j - 1) < 0) {
+ // top-left pixel
+ (*binary_cloud)(i, j).intensity = c_intensity;
}
- else
- {
- if ((j - 1) < 0)
- {
- //top-row, check on the left of pixel to assign a new label or not
- int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
- if (left)
- {
- //Nothing found on the left, check bigger window
+ else {
+ if ((j - 1) < 0) {
+ // top-row, check on the left of pixel to assign a new label or not
+ int left = check((*binary_cloud)(i - 1, j),
+ (*binary_cloud)(i, j),
+ c_intensity,
+ object_cluster_tolerance_);
+ if (left) {
+ // Nothing found on the left, check bigger window
bool found = false;
- for (int kk = 2; kk < wsize && !found; kk++)
- {
+ for (int kk = 2; kk < wsize && !found; kk++) {
if ((i - kk) < 0)
continue;
- int left = check ((*binary_cloud) (i - kk, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
- if (left == 0)
- {
+ int left = check((*binary_cloud)(i - kk, j),
+ (*binary_cloud)(i, j),
+ c_intensity,
+ object_cluster_tolerance_);
+ if (left == 0) {
found = true;
}
}
- if (!found)
- {
+ if (!found) {
c_intensity += intensity_incr;
- (*binary_cloud) (i, j).intensity = c_intensity;
+ (*binary_cloud)(i, j).intensity = c_intensity;
}
-
}
}
- else
- {
- if ((i - 1) == 0)
- {
- //check only top
- int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
- if (top)
- {
+ else {
+ if ((i - 1) == 0) {
+ // check only top
+ int top = check((*binary_cloud)(i, j - 1),
+ (*binary_cloud)(i, j),
+ c_intensity,
+ object_cluster_tolerance_);
+ if (top) {
bool found = false;
- for (int kk = 2; kk < wsize && !found; kk++)
- {
+ for (int kk = 2; kk < wsize && !found; kk++) {
if ((j - kk) < 0)
continue;
- int top = check ((*binary_cloud) (i, j - kk), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
- if (top == 0)
- {
+ int top = check((*binary_cloud)(i, j - kk),
+ (*binary_cloud)(i, j),
+ c_intensity,
+ object_cluster_tolerance_);
+ if (top == 0) {
found = true;
}
}
- if (!found)
- {
+ if (!found) {
c_intensity += intensity_incr;
- (*binary_cloud) (i, j).intensity = c_intensity;
+ (*binary_cloud)(i, j).intensity = c_intensity;
}
}
-
}
- else
- {
- //check left and top
- int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
- int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
-
- if (left == 0 && top == 0)
- {
- //both top and left had labels, check if they are different
- //if they are, take the smallest one and mark labels to be connected..
-
- if ((*binary_cloud) (i - 1, j).intensity != (*binary_cloud) (i, j - 1).intensity)
- {
- float smaller_intensity = (*binary_cloud) (i - 1, j).intensity;
- float bigger_intensity = (*binary_cloud) (i, j - 1).intensity;
-
- if ((*binary_cloud) (i - 1, j).intensity > (*binary_cloud) (i, j - 1).intensity)
- {
- smaller_intensity = (*binary_cloud) (i, j - 1).intensity;
- bigger_intensity = (*binary_cloud) (i - 1, j).intensity;
+ else {
+ // check left and top
+ int left = check((*binary_cloud)(i - 1, j),
+ (*binary_cloud)(i, j),
+ c_intensity,
+ object_cluster_tolerance_);
+ int top = check((*binary_cloud)(i, j - 1),
+ (*binary_cloud)(i, j),
+ c_intensity,
+ object_cluster_tolerance_);
+
+ if (left == 0 && top == 0) {
+ // both top and left had labels, check if they are different
+ // if they are, take the smallest one and mark labels to be
+ // connected..
+
+ if ((*binary_cloud)(i - 1, j).intensity !=
+ (*binary_cloud)(i, j - 1).intensity) {
+ float smaller_intensity = (*binary_cloud)(i - 1, j).intensity;
+ float bigger_intensity = (*binary_cloud)(i, j - 1).intensity;
+
+ if ((*binary_cloud)(i - 1, j).intensity >
+ (*binary_cloud)(i, j - 1).intensity) {
+ smaller_intensity = (*binary_cloud)(i, j - 1).intensity;
+ bigger_intensity = (*binary_cloud)(i - 1, j).intensity;
}
connected_labels[bigger_intensity] = smaller_intensity;
- (*binary_cloud) (i, j).intensity = smaller_intensity;
+ (*binary_cloud)(i, j).intensity = smaller_intensity;
}
}
- if (left == 1 && top == 1)
- {
- //if none had labels, increment c_intensity
- //search first on bigger window
+ if (left == 1 && top == 1) {
+ // if none had labels, increment c_intensity
+ // search first on bigger window
bool found = false;
- for (int dist = 2; dist < wsize && !found; dist++)
- {
+ for (int dist = 2; dist < wsize && !found; dist++) {
if (((i - dist) < 0) || ((j - dist) < 0))
continue;
- int left = check ((*binary_cloud) (i - dist, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
- int top = check ((*binary_cloud) (i, j - dist), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
-
- if (left == 0 && top == 0)
- {
- if ((*binary_cloud) (i - dist, j).intensity != (*binary_cloud) (i, j - dist).intensity)
- {
- float smaller_intensity = (*binary_cloud) (i - dist, j).intensity;
- float bigger_intensity = (*binary_cloud) (i, j - dist).intensity;
-
- if ((*binary_cloud) (i - dist, j).intensity > (*binary_cloud) (i, j - dist).intensity)
- {
- smaller_intensity = (*binary_cloud) (i, j - dist).intensity;
- bigger_intensity = (*binary_cloud) (i - dist, j).intensity;
+ int left = check((*binary_cloud)(i - dist, j),
+ (*binary_cloud)(i, j),
+ c_intensity,
+ object_cluster_tolerance_);
+ int top = check((*binary_cloud)(i, j - dist),
+ (*binary_cloud)(i, j),
+ c_intensity,
+ object_cluster_tolerance_);
+
+ if (left == 0 && top == 0) {
+ if ((*binary_cloud)(i - dist, j).intensity !=
+ (*binary_cloud)(i, j - dist).intensity) {
+ float smaller_intensity =
+ (*binary_cloud)(i - dist, j).intensity;
+ float bigger_intensity = (*binary_cloud)(i, j - dist).intensity;
+
+ if ((*binary_cloud)(i - dist, j).intensity >
+ (*binary_cloud)(i, j - dist).intensity) {
+ smaller_intensity = (*binary_cloud)(i, j - dist).intensity;
+ bigger_intensity = (*binary_cloud)(i - dist, j).intensity;
}
connected_labels[bigger_intensity] = smaller_intensity;
- (*binary_cloud) (i, j).intensity = smaller_intensity;
+ (*binary_cloud)(i, j).intensity = smaller_intensity;
found = true;
}
}
- else if (left == 0 || top == 0)
- {
- //one had label
+ else if (left == 0 || top == 0) {
+ // one had label
found = true;
}
}
- if (!found)
- {
- //none had label in the bigger window
+ if (!found) {
+ // none had label in the bigger window
c_intensity += intensity_incr;
- (*binary_cloud) (i, j).intensity = c_intensity;
+ (*binary_cloud)(i, j).intensity = c_intensity;
}
}
}
}
}
-
}
}
}
{
std::map<float, float>::iterator it;
- for (int i = 0; i < int (binary_cloud->width); i++)
- {
- for (int j = 0; j < int (binary_cloud->height); j++)
- {
- if (binary_cloud->at (i, j).intensity != 0)
- {
- //check if this is a root label...
- it = connected_labels.find (binary_cloud->at (i, j).intensity);
- while (it != connected_labels.end ())
- {
- //the label is on the list, change pixel intensity until it has a root label
- (*binary_cloud) (i, j).intensity = (*it).second;
- it = connected_labels.find (binary_cloud->at (i, j).intensity);
+ for (int i = 0; i < int(binary_cloud->width); i++) {
+ for (int j = 0; j < int(binary_cloud->height); j++) {
+ if (binary_cloud->at(i, j).intensity != 0) {
+ // check if this is a root label...
+ it = connected_labels.find(binary_cloud->at(i, j).intensity);
+ while (it != connected_labels.end()) {
+ // the label is on the list, change pixel intensity until it
+ // has a root label
+ (*binary_cloud)(i, j).intensity = (*it).second;
+ it = connected_labels.find(binary_cloud->at(i, j).intensity);
}
std::map<float, pcl::PointIndices>::iterator it_indices;
- it_indices = clusters_map.find (binary_cloud->at (i, j).intensity);
- if (it_indices == clusters_map.end ())
- {
+ it_indices = clusters_map.find(binary_cloud->at(i, j).intensity);
+ if (it_indices == clusters_map.end()) {
pcl::PointIndices indices;
- clusters_map[binary_cloud->at (i, j).intensity] = indices;
+ clusters_map[binary_cloud->at(i, j).intensity] = indices;
}
- clusters_map[binary_cloud->at (i, j).intensity].indices.push_back (static_cast<int> (j * binary_cloud->width + i));
+ clusters_map[binary_cloud->at(i, j).intensity].indices.push_back(
+ static_cast<int>(j * binary_cloud->width + i));
}
}
}
}
- clusters.resize (clusters_map.size ());
+ clusters.resize(clusters_map.size());
int k = 0;
- for (const auto &cluster : clusters_map)
- {
+ for (const auto& cluster : clusters_map) {
- if (int (cluster.second.indices.size ()) >= object_cluster_min_size_)
- {
+ if (int(cluster.second.indices.size()) >= object_cluster_min_size_) {
- clusters[k] = CloudPtr (new Cloud ());
- pcl::copyPointCloud (*input_, cluster.second.indices, *clusters[k]);
+ clusters[k] = CloudPtr(new Cloud());
+ pcl::copyPointCloud(*input_, cluster.second.indices, *clusters[k]);
k++;
indices_clusters_.push_back(cluster.second);
}
}
- clusters.resize (k);
-
+ clusters.resize(k);
}
-template<typename PointType> void
-pcl::apps::DominantPlaneSegmentation<PointType>::compute (std::vector<CloudPtr> & clusters)
+template <typename PointType>
+void
+pcl::apps::DominantPlaneSegmentation<PointType>::compute(
+ std::vector<CloudPtr>& clusters)
{
// Has the input dataset been set already?
- if (!input_)
- {
- PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
+ if (!input_) {
+ PCL_WARN("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
CloudConstPtr cloud_;
- CloudPtr cloud_filtered_ (new Cloud ());
- CloudPtr cloud_downsampled_ (new Cloud ());
- pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
- pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
- pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
- CloudPtr table_projected_ (new Cloud ());
- CloudPtr table_hull_ (new Cloud ());
- CloudPtr cloud_objects_ (new Cloud ());
- CloudPtr cluster_object_ (new Cloud ());
-
- typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
- typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
- clusters_tree_->setEpsilon (1);
+ CloudPtr cloud_filtered_(new Cloud());
+ CloudPtr cloud_downsampled_(new Cloud());
+ pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_(new pcl::PointCloud<pcl::Normal>());
+ pcl::PointIndices::Ptr table_inliers_(new pcl::PointIndices());
+ pcl::ModelCoefficients::Ptr table_coefficients_(new pcl::ModelCoefficients());
+ CloudPtr table_projected_(new Cloud());
+ CloudPtr table_hull_(new Cloud());
+ CloudPtr cloud_objects_(new Cloud());
+ CloudPtr cluster_object_(new Cloud());
+
+ typename pcl::search::KdTree<PointType>::Ptr normals_tree_(
+ new pcl::search::KdTree<PointType>);
+ typename pcl::search::KdTree<PointType>::Ptr clusters_tree_(
+ new pcl::search::KdTree<PointType>);
+ clusters_tree_->setEpsilon(1);
// Normal estimation parameters
- n3d_.setKSearch (k_);
- n3d_.setSearchMethod (normals_tree_);
+ n3d_.setKSearch(k_);
+ n3d_.setSearchMethod(normals_tree_);
// Table model fitting parameters
- seg_.setDistanceThreshold (sac_distance_threshold_);
- seg_.setMaxIterations (2000);
- seg_.setNormalDistanceWeight (0.1);
- seg_.setOptimizeCoefficients (true);
- seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
- seg_.setMethodType (pcl::SAC_RANSAC);
- seg_.setProbability (0.99);
+ seg_.setDistanceThreshold(sac_distance_threshold_);
+ seg_.setMaxIterations(2000);
+ seg_.setNormalDistanceWeight(0.1);
+ seg_.setOptimizeCoefficients(true);
+ seg_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
+ seg_.setMethodType(pcl::SAC_RANSAC);
+ seg_.setProbability(0.99);
- proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
- bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
+ proj_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
+ bb_cluster_proj_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
- prism_.setHeightLimits (object_min_height_, object_max_height_);
+ prism_.setHeightLimits(object_min_height_, object_max_height_);
// Clustering parameters
- cluster_.setClusterTolerance (object_cluster_tolerance_);
- cluster_.setMinClusterSize (object_cluster_min_size_);
- cluster_.setSearchMethod (clusters_tree_);
+ cluster_.setClusterTolerance(object_cluster_tolerance_);
+ cluster_.setMinClusterSize(object_cluster_min_size_);
+ cluster_.setSearchMethod(clusters_tree_);
// ---[ PassThroughFilter
- pass_.setFilterLimits (float (min_z_bounds_), float (max_z_bounds_));
- pass_.setFilterFieldName ("z");
- pass_.setInputCloud (input_);
- pass_.filter (*cloud_filtered_);
-
- if (int (cloud_filtered_->points.size ()) < k_)
- {
- PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
- cloud_filtered_->points.size ());
+ pass_.setFilterLimits(float(min_z_bounds_), float(max_z_bounds_));
+ pass_.setFilterFieldName("z");
+ pass_.setInputCloud(input_);
+ pass_.filter(*cloud_filtered_);
+
+ if (int(cloud_filtered_->points.size()) < k_) {
+ PCL_WARN("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
+ cloud_filtered_->points.size());
return;
}
// Downsample the point cloud
- grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
- grid_.setDownsampleAllData (false);
- grid_.setInputCloud (cloud_filtered_);
- grid_.filter (*cloud_downsampled_);
-
- PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering (%f -> %f): %lu out of %lu\n",
- min_z_bounds_, max_z_bounds_, cloud_downsampled_->points.size (), input_->points.size ());
+ grid_.setLeafSize(downsample_leaf_, downsample_leaf_, downsample_leaf_);
+ grid_.setDownsampleAllData(false);
+ grid_.setInputCloud(cloud_filtered_);
+ grid_.filter(*cloud_downsampled_);
+
+ PCL_INFO("[DominantPlaneSegmentation] Number of points left after filtering "
+ "(%f -> %f): %lu out of %lu\n",
+ min_z_bounds_,
+ max_z_bounds_,
+ cloud_downsampled_->points.size(),
+ input_->points.size());
// ---[ Estimate the point normals
- n3d_.setInputCloud (cloud_downsampled_);
- n3d_.compute (*cloud_normals_);
+ n3d_.setInputCloud(cloud_downsampled_);
+ n3d_.compute(*cloud_normals_);
- PCL_INFO ("[DominantPlaneSegmentation] %lu normals estimated. \n", cloud_normals_->points.size ());
+ PCL_INFO("[DominantPlaneSegmentation] %lu normals estimated. \n",
+ cloud_normals_->points.size());
// ---[ Perform segmentation
- seg_.setInputCloud (cloud_downsampled_);
- seg_.setInputNormals (cloud_normals_);
- seg_.segment (*table_inliers_, *table_coefficients_);
+ seg_.setInputCloud(cloud_downsampled_);
+ seg_.setInputNormals(cloud_normals_);
+ seg_.segment(*table_inliers_, *table_coefficients_);
- if (table_inliers_->indices.empty ())
- {
- PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
+ if (table_inliers_->indices.empty()) {
+ PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
- proj_.setInputCloud (cloud_downsampled_);
- proj_.setIndices (table_inliers_);
- proj_.setModelCoefficients (table_coefficients_);
- proj_.filter (*table_projected_);
+ proj_.setInputCloud(cloud_downsampled_);
+ proj_.setIndices(table_inliers_);
+ proj_.setModelCoefficients(table_coefficients_);
+ proj_.filter(*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
- CloudPtr table_hull (new Cloud ());
- hull_.setInputCloud (table_projected_);
- hull_.reconstruct (*table_hull, polygons);
+ CloudPtr table_hull(new Cloud());
+ hull_.setInputCloud(table_projected_);
+ hull_.reconstruct(*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
- Eigen::Vector4f vp (0, 0, 0, 0);
+ Eigen::Vector4f vp(0, 0, 0, 0);
// See if we need to flip any plane normals
- vp -= table_hull->points[0].getVector4fMap ();
+ vp -= table_hull->points[0].getVector4fMap();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
- float cos_theta = vp.dot (model_coefficients);
+ float cos_theta = vp.dot(model_coefficients);
// Flip the plane normal
- if (cos_theta < 0)
- {
+ if (cos_theta < 0) {
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
- model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
+ model_coefficients[3] =
+ -1 * (model_coefficients.dot(table_hull->points[0].getVector4fMap()));
}
- //Set table_coeffs
+ // Set table_coeffs
table_coeffs_ = model_coefficients;
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices;
- prism_.setInputCloud (cloud_downsampled_);
- prism_.setInputPlanarHull (table_hull);
- prism_.segment (cloud_object_indices);
+ prism_.setInputCloud(cloud_downsampled_);
+ prism_.setInputPlanarHull(table_hull);
+ prism_.segment(cloud_object_indices);
pcl::ExtractIndices<PointType> extract_object_indices;
- extract_object_indices.setInputCloud (cloud_downsampled_);
- extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
- extract_object_indices.filter (*cloud_objects_);
+ extract_object_indices.setInputCloud(cloud_downsampled_);
+ extract_object_indices.setIndices(
+ pcl::make_shared<const pcl::PointIndices>(cloud_object_indices));
+ extract_object_indices.filter(*cloud_objects_);
- if (cloud_objects_->points.empty ())
+ if (cloud_objects_->points.empty())
return;
- //down_.reset(new Cloud(*cloud_downsampled_));
+ // down_.reset(new Cloud(*cloud_downsampled_));
// ---[ Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clusters2;
- cluster_.setInputCloud (cloud_downsampled_);
- cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
- cluster_.extract (clusters2);
+ cluster_.setInputCloud(cloud_downsampled_);
+ cluster_.setIndices(pcl::make_shared<const pcl::PointIndices>(cloud_object_indices));
+ cluster_.extract(clusters2);
- PCL_INFO ("[DominantPlaneSegmentation::compute()] Number of clusters found matching the given constraints: %lu.\n",
- clusters2.size ());
+ PCL_INFO("[DominantPlaneSegmentation::compute()] Number of clusters found matching "
+ "the given constraints: %lu.\n",
+ clusters2.size());
- clusters.resize (clusters2.size ());
+ clusters.resize(clusters2.size());
- for (std::size_t i = 0; i < clusters2.size (); ++i)
- {
- clusters[i] = CloudPtr (new Cloud ());
- pcl::copyPointCloud (*cloud_downsampled_, clusters2[i].indices, *clusters[i]);
+ for (std::size_t i = 0; i < clusters2.size(); ++i) {
+ clusters[i] = CloudPtr(new Cloud());
+ pcl::copyPointCloud(*cloud_downsampled_, clusters2[i].indices, *clusters[i]);
}
}
-template<typename PointType>
+template <typename PointType>
void
-pcl::apps::DominantPlaneSegmentation<PointType>::compute_full (std::vector<CloudPtr> & clusters)
+pcl::apps::DominantPlaneSegmentation<PointType>::compute_full(
+ std::vector<CloudPtr>& clusters)
{
// Has the input dataset been set already?
- if (!input_)
- {
- PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
+ if (!input_) {
+ PCL_WARN("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
CloudConstPtr cloud_;
- CloudPtr cloud_filtered_ (new Cloud ());
- CloudPtr cloud_downsampled_ (new Cloud ());
- pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
- pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
- pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
- CloudPtr table_projected_ (new Cloud ());
- CloudPtr table_hull_ (new Cloud ());
- CloudPtr cloud_objects_ (new Cloud ());
- CloudPtr cluster_object_ (new Cloud ());
-
- typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
- typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
- clusters_tree_->setEpsilon (1);
+ CloudPtr cloud_filtered_(new Cloud());
+ CloudPtr cloud_downsampled_(new Cloud());
+ pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_(new pcl::PointCloud<pcl::Normal>());
+ pcl::PointIndices::Ptr table_inliers_(new pcl::PointIndices());
+ pcl::ModelCoefficients::Ptr table_coefficients_(new pcl::ModelCoefficients());
+ CloudPtr table_projected_(new Cloud());
+ CloudPtr table_hull_(new Cloud());
+ CloudPtr cloud_objects_(new Cloud());
+ CloudPtr cluster_object_(new Cloud());
+
+ typename pcl::search::KdTree<PointType>::Ptr normals_tree_(
+ new pcl::search::KdTree<PointType>);
+ typename pcl::search::KdTree<PointType>::Ptr clusters_tree_(
+ new pcl::search::KdTree<PointType>);
+ clusters_tree_->setEpsilon(1);
// Normal estimation parameters
- n3d_.setKSearch (10);
- n3d_.setSearchMethod (normals_tree_);
+ n3d_.setKSearch(10);
+ n3d_.setSearchMethod(normals_tree_);
// Table model fitting parameters
- seg_.setDistanceThreshold (sac_distance_threshold_);
- seg_.setMaxIterations (2000);
- seg_.setNormalDistanceWeight (0.1);
- seg_.setOptimizeCoefficients (true);
- seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
- seg_.setMethodType (pcl::SAC_MSAC);
- seg_.setProbability (0.98);
+ seg_.setDistanceThreshold(sac_distance_threshold_);
+ seg_.setMaxIterations(2000);
+ seg_.setNormalDistanceWeight(0.1);
+ seg_.setOptimizeCoefficients(true);
+ seg_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
+ seg_.setMethodType(pcl::SAC_MSAC);
+ seg_.setProbability(0.98);
- proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
- bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
+ proj_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
+ bb_cluster_proj_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
- prism_.setHeightLimits (object_min_height_, object_max_height_);
+ prism_.setHeightLimits(object_min_height_, object_max_height_);
// Clustering parameters
- cluster_.setClusterTolerance (object_cluster_tolerance_);
- cluster_.setMinClusterSize (object_cluster_min_size_);
- cluster_.setSearchMethod (clusters_tree_);
+ cluster_.setClusterTolerance(object_cluster_tolerance_);
+ cluster_.setMinClusterSize(object_cluster_min_size_);
+ cluster_.setSearchMethod(clusters_tree_);
// ---[ PassThroughFilter
- pass_.setFilterLimits (float (min_z_bounds_), float (max_z_bounds_));
- pass_.setFilterFieldName ("z");
- pass_.setInputCloud (input_);
- pass_.filter (*cloud_filtered_);
-
- if (int (cloud_filtered_->points.size ()) < k_)
- {
- PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
- cloud_filtered_->points.size ());
+ pass_.setFilterLimits(float(min_z_bounds_), float(max_z_bounds_));
+ pass_.setFilterFieldName("z");
+ pass_.setInputCloud(input_);
+ pass_.filter(*cloud_filtered_);
+
+ if (int(cloud_filtered_->points.size()) < k_) {
+ PCL_WARN("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
+ cloud_filtered_->points.size());
return;
}
// Downsample the point cloud
- grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
- grid_.setDownsampleAllData (false);
- grid_.setInputCloud (cloud_filtered_);
- grid_.filter (*cloud_downsampled_);
-
- PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering&downsampling (%f -> %f): %lu out of %lu\n",
- min_z_bounds_, max_z_bounds_, cloud_downsampled_->points.size (), input_->points.size ());
+ grid_.setLeafSize(downsample_leaf_, downsample_leaf_, downsample_leaf_);
+ grid_.setDownsampleAllData(false);
+ grid_.setInputCloud(cloud_filtered_);
+ grid_.filter(*cloud_downsampled_);
+
+ PCL_INFO("[DominantPlaneSegmentation] Number of points left after "
+ "filtering&downsampling (%f -> %f): %lu out of %lu\n",
+ min_z_bounds_,
+ max_z_bounds_,
+ cloud_downsampled_->points.size(),
+ input_->points.size());
// ---[ Estimate the point normals
- n3d_.setInputCloud (cloud_downsampled_);
- n3d_.compute (*cloud_normals_);
+ n3d_.setInputCloud(cloud_downsampled_);
+ n3d_.compute(*cloud_normals_);
- PCL_INFO ("[DominantPlaneSegmentation] %lu normals estimated. \n", cloud_normals_->points.size ());
+ PCL_INFO("[DominantPlaneSegmentation] %lu normals estimated. \n",
+ cloud_normals_->points.size());
// ---[ Perform segmentation
- seg_.setInputCloud (cloud_downsampled_);
- seg_.setInputNormals (cloud_normals_);
- seg_.segment (*table_inliers_, *table_coefficients_);
+ seg_.setInputCloud(cloud_downsampled_);
+ seg_.setInputNormals(cloud_normals_);
+ seg_.segment(*table_inliers_, *table_coefficients_);
- if (table_inliers_->indices.empty ())
- {
- PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
+ if (table_inliers_->indices.empty()) {
+ PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
- proj_.setInputCloud (cloud_downsampled_);
- proj_.setIndices (table_inliers_);
- proj_.setModelCoefficients (table_coefficients_);
- proj_.filter (*table_projected_);
+ proj_.setInputCloud(cloud_downsampled_);
+ proj_.setIndices(table_inliers_);
+ proj_.setModelCoefficients(table_coefficients_);
+ proj_.filter(*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
- CloudPtr table_hull (new Cloud ());
- hull_.setInputCloud (table_projected_);
- hull_.reconstruct (*table_hull, polygons);
+ CloudPtr table_hull(new Cloud());
+ hull_.setInputCloud(table_projected_);
+ hull_.reconstruct(*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
- Eigen::Vector4f vp (0, 0, 0, 0);
+ Eigen::Vector4f vp(0, 0, 0, 0);
// See if we need to flip any plane normals
- vp -= table_hull->points[0].getVector4fMap ();
+ vp -= table_hull->points[0].getVector4fMap();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
- float cos_theta = vp.dot (model_coefficients);
+ float cos_theta = vp.dot(model_coefficients);
// Flip the plane normal
- if (cos_theta < 0)
- {
+ if (cos_theta < 0) {
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
- model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
+ model_coefficients[3] =
+ -1 * (model_coefficients.dot(table_hull->points[0].getVector4fMap()));
}
- //Set table_coeffs
+ // Set table_coeffs
table_coeffs_ = model_coefficients;
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices;
- prism_.setInputCloud (cloud_filtered_);
- prism_.setInputPlanarHull (table_hull);
- prism_.segment (cloud_object_indices);
+ prism_.setInputCloud(cloud_filtered_);
+ prism_.setInputPlanarHull(table_hull);
+ prism_.segment(cloud_object_indices);
pcl::ExtractIndices<PointType> extract_object_indices;
- extract_object_indices.setInputCloud (cloud_downsampled_);
- extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
- extract_object_indices.filter (*cloud_objects_);
+ extract_object_indices.setInputCloud(cloud_downsampled_);
+ extract_object_indices.setIndices(
+ pcl::make_shared<const pcl::PointIndices>(cloud_object_indices));
+ extract_object_indices.filter(*cloud_objects_);
- if (cloud_objects_->points.empty ())
+ if (cloud_objects_->points.empty())
return;
// ---[ Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clusters2;
- cluster_.setInputCloud (cloud_filtered_);
- cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
- cluster_.extract (clusters2);
+ cluster_.setInputCloud(cloud_filtered_);
+ cluster_.setIndices(pcl::make_shared<const pcl::PointIndices>(cloud_object_indices));
+ cluster_.extract(clusters2);
- PCL_INFO ("[DominantPlaneSegmentation::compute_full()] Number of clusters found matching the given constraints: %lu.\n",
- clusters2.size ());
+ PCL_INFO("[DominantPlaneSegmentation::compute_full()] Number of clusters found "
+ "matching the given constraints: %lu.\n",
+ clusters2.size());
- clusters.resize (clusters2.size ());
+ clusters.resize(clusters2.size());
- for (std::size_t i = 0; i < clusters2.size (); ++i)
- {
- clusters[i] = CloudPtr (new Cloud ());
- pcl::copyPointCloud (*cloud_filtered_, clusters2[i].indices, *clusters[i]);
+ for (std::size_t i = 0; i < clusters2.size(); ++i) {
+ clusters[i] = CloudPtr(new Cloud());
+ pcl::copyPointCloud(*cloud_filtered_, clusters2[i].indices, *clusters[i]);
}
}
-#define PCL_INSTANTIATE_DominantPlaneSegmentation(T) template class PCL_EXPORTS pcl::apps::DominantPlaneSegmentation<T>;
+#define PCL_INSTANTIATE_DominantPlaneSegmentation(T) \
+ template class PCL_EXPORTS pcl::apps::DominantPlaneSegmentation<T>;
/*
* Software License Agreement (BSD License)
- *
+ *
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
- *
+ *
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
- * are met:
- *
+ * are met:
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include <ui_manual_registration.h>
-
-// QT
-#include <QMainWindow>
-#include <QMutex>
-#include <QTimer>
-
-// PCL
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
-
-#include <pcl/common/common.h>
#include <pcl/common/angles.h>
+#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <pcl/common/transforms.h>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/io/pcd_io.h>
-
+#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
-#include <pcl/registration/transformation_estimation_svd.h>
+#include <QMainWindow>
+#include <QMutex>
+#include <QTimer>
+#include <ui_manual_registration.h>
using PointT = pcl::PointXYZRGBA;
// Useful macros
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
-
-namespace Ui
-{
- class MainWindow;
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
+
+namespace Ui {
+class MainWindow;
}
-class ManualRegistration : public QMainWindow
-{
+class ManualRegistration : public QMainWindow {
Q_OBJECT
- public:
- using Cloud = pcl::PointCloud<PointT>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
-
- ManualRegistration ();
-
- ~ManualRegistration ()
- {
- }
-
- void
- setSrcCloud (pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_src)
- {
- cloud_src_ = std::move(cloud_src);
- cloud_src_present_ = true;
- }
- void
- setDstCloud (pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_dst)
- {
- cloud_dst_ = std::move(cloud_dst);
- cloud_dst_present_ = true;
- }
-
- void
- SourcePointPickCallback (const pcl::visualization::PointPickingEvent& event, void*);
- void
- DstPointPickCallback (const pcl::visualization::PointPickingEvent& event, void*);
-
- protected:
- pcl::visualization::PCLVisualizer::Ptr vis_src_;
- pcl::visualization::PCLVisualizer::Ptr vis_dst_;
-
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_src_;
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_dst_;
-
- QMutex mtx_;
- QMutex vis_mtx_;
- Ui::MainWindow *ui_;
- QTimer *vis_timer_;
-
- bool cloud_src_present_;
- bool cloud_src_modified_;
- bool cloud_dst_present_;
- bool cloud_dst_modified_;
-
- bool src_point_selected_;
- bool dst_point_selected_;
-
- pcl::PointXYZ src_point_;
- pcl::PointXYZ dst_point_;
-
- pcl::PointCloud<pcl::PointXYZ> src_pc_;
- pcl::PointCloud<pcl::PointXYZ> dst_pc_;
-
- Eigen::Matrix4f transform_;
-
- public Q_SLOTS:
- void
- confirmSrcPointPressed();
- void
- confirmDstPointPressed();
- void
- calculatePressed();
- void
- clearPressed();
- void
- orthoChanged(int state);
- void
- applyTransformPressed();
- void
- refinePressed();
- void
- undoPressed();
- void
- safePressed();
-
- private Q_SLOTS:
- void
- timeoutSlot();
-
+public:
+ using Cloud = pcl::PointCloud<PointT>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ ManualRegistration();
+
+ ~ManualRegistration() {}
+
+ void
+ setSrcCloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_src)
+ {
+ cloud_src_ = std::move(cloud_src);
+ cloud_src_present_ = true;
+ }
+ void
+ setDstCloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_dst)
+ {
+ cloud_dst_ = std::move(cloud_dst);
+ cloud_dst_present_ = true;
+ }
+
+ void
+ SourcePointPickCallback(const pcl::visualization::PointPickingEvent& event, void*);
+ void
+ DstPointPickCallback(const pcl::visualization::PointPickingEvent& event, void*);
+
+protected:
+ pcl::visualization::PCLVisualizer::Ptr vis_src_;
+ pcl::visualization::PCLVisualizer::Ptr vis_dst_;
+
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_src_;
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_dst_;
+
+ QMutex mtx_;
+ QMutex vis_mtx_;
+ Ui::MainWindow* ui_;
+ QTimer* vis_timer_;
+
+ bool cloud_src_present_;
+ bool cloud_src_modified_;
+ bool cloud_dst_present_;
+ bool cloud_dst_modified_;
+
+ bool src_point_selected_;
+ bool dst_point_selected_;
+
+ pcl::PointXYZ src_point_;
+ pcl::PointXYZ dst_point_;
+
+ pcl::PointCloud<pcl::PointXYZ> src_pc_;
+ pcl::PointCloud<pcl::PointXYZ> dst_pc_;
+
+ Eigen::Matrix4f transform_;
+
+public Q_SLOTS:
+ void
+ confirmSrcPointPressed();
+ void
+ confirmDstPointPressed();
+ void
+ calculatePressed();
+ void
+ clearPressed();
+ void
+ orthoChanged(int state);
+ void
+ applyTransformPressed();
+ void
+ refinePressed();
+ void
+ undoPressed();
+ void
+ safePressed();
+
+private Q_SLOTS:
+ void
+ timeoutSlot();
};
#pragma once
-#include <cstdlib>
-#include <cfloat>
-#include <algorithm>
-#include <boost/foreach.hpp>
+#include <pcl/io/pcd_io.h>
+#include <pcl/kdtree/kdtree_flann.h>
-namespace pcl
-{
- /**
- * \brief Nearest neighbor search based classification of PCL point type features.
- * FLANN is used to identify a neighborhood, based on which different scoring schemes
- * can be employed to obtain likelihood values for a specified list of classes.
- * \author Zoltan Csaba Marton
- */
- template <typename PointT>
- class NNClassification
- {
- private:
+namespace pcl {
- typename pcl::KdTree<PointT>::Ptr tree_;
+/**
+ * \brief Nearest neighbor search based classification of PCL point type features.
+ * FLANN is used to identify a neighborhood, based on which different scoring schemes
+ * can be employed to obtain likelihood values for a specified list of classes.
+ * \author Zoltan Csaba Marton
+ */
+template <typename PointT>
+class NNClassification {
+private:
+ typename pcl::KdTree<PointT>::Ptr tree_;
- /** \brief List of class labels */
- std::vector<std::string> classes_;
- /** \brief The index in the class labels list for all the training examples */
- std::vector<int> labels_idx_;
+ /** \brief List of class labels */
+ std::vector<std::string> classes_;
+ /** \brief The index in the class labels list for all the training examples */
+ std::vector<int> labels_idx_;
- public:
+public:
+ NNClassification() : tree_() {}
- NNClassification () : tree_ () {}
+ /** \brief Result is a list of class labels and scores */
+ using Result = std::pair<std::vector<std::string>, std::vector<float>>;
+ using ResultPtr = std::shared_ptr<Result>;
- /** \brief Result is a list of class labels and scores */
- using Result = std::pair<std::vector<std::string>, std::vector<float> >;
- using ResultPtr = std::shared_ptr<Result>;
+ // TODO setIndices method, distance metrics and reset tree
- // TODO setIndices method, distance metrics and reset tree
+ /**
+ * \brief Setting the training features.
+ * \param[in] features the training features
+ */
+ void
+ setTrainingFeatures(const typename pcl::PointCloud<PointT>::ConstPtr& features)
+ {
+ // Do not limit the number of dimensions used in the tree
+ typename pcl::CustomPointRepresentation<PointT>::Ptr cpr(
+ new pcl::CustomPointRepresentation<PointT>(INT_MAX, 0));
+ tree_.reset(new pcl::KdTreeFLANN<PointT>);
+ tree_->setPointRepresentation(cpr);
+ tree_->setInputCloud(features);
+ }
- /** \brief Setting the training features.
- * \param[in] features the training features
- */
- void
- setTrainingFeatures (const typename pcl::PointCloud<PointT>::ConstPtr &features)
- {
- // Do not limit the number of dimensions used in the tree
- typename pcl::CustomPointRepresentation<PointT>::Ptr cpr (new pcl::CustomPointRepresentation<PointT> (INT_MAX, 0));
- tree_.reset (new pcl::KdTreeFLANN<PointT>);
- tree_->setPointRepresentation (cpr);
- tree_->setInputCloud (features);
- }
+ /**
+ * \brief Updating the labels for each training example.
+ * \param classes the class labels
+ * \param labels_idx the index in the class labels list for each training example
+ */
+ void
+ setTrainingLabelIndicesAndLUT(const std::vector<std::string>& classes,
+ const std::vector<int>& labels_idx)
+ {
+ // TODO check if min/max index is inside classes?
+ classes_ = classes;
+ labels_idx_ = labels_idx;
+ }
- /** \brief Updating the labels for each training example.
- * \param classes the class labels
- * \param labels_idx the index in the class labels list for each training example
- */
- void
- setTrainingLabelIndicesAndLUT (const std::vector<std::string> &classes, const std::vector<int> &labels_idx)
- {
- // TODO check if min/max index is inside classes?
- classes_ = classes;
- labels_idx_ = labels_idx;
- }
+ /**
+ * \brief Setting the labels for each training example.
+ * The unique labels from the list are stored as the class labels, and
+ * for each training example an index pointing to these labels is stored.
+ * \note See the setTrainingLabelIndicesAndLUT method for easily re-labeling.
+ * \param labels the class label for each training example
+ */
+ void
+ setTrainingLabels(const std::vector<std::string>& labels)
+ {
+ // Create a list of unique labels
+ classes_ = labels;
+ std::sort(classes_.begin(), classes_.end());
+ classes_.erase(std::unique(classes_.begin(), classes_.end()), classes_.end());
- /** \brief Setting the labels for each training example.
- * The unique labels from the list are stored as the class labels, and
- * for each training example an index pointing to these labels is stored.
- * \note See the setTrainingLabelIndicesAndLUT method for easily re-labeling.
- * \param labels the class label for each training example
- */
- void
- setTrainingLabels (const std::vector<std::string> &labels)
- {
- // Create a list of unique labels
- classes_ = labels;
- std::sort (classes_.begin(), classes_.end());
- classes_.erase (std::unique (classes_.begin(), classes_.end()), classes_.end());
+ // Save the mapping from labels to indices in the class list
+ std::map<std::string, int> label2idx;
+ for (std::size_t i = 0; i < classes_.size(); ++i) {
+ label2idx[classes_[i]] = i;
+ }
- // Save the mapping from labels to indices in the class list
- std::map<std::string, int> label2idx;
- for (std::size_t i = 0; i < classes_.size(); ++i)
- {
- label2idx[classes_[i]] = i;
- }
+ // Create a list holding the class index of each label
+ labels_idx_.reserve(labels.size());
+ for (const auto& s : labels) {
+ labels_idx_.push_back(label2idx[s]);
+ }
+ }
- // Create a list holding the class index of each label
- labels_idx_.reserve (labels.size ());
- for (const auto &s : labels)
- {
- labels_idx_.push_back (label2idx[s]);
- }
- }
+ /**
+ * \brief Load the list of training examples and corresponding labels.
+ * \param file_name PCD file containing the training features
+ * \param labels_file_name the class label for each training example
+ * \return true on success, false on failure (read error or number of entries don't
+ * match)
+ */
+ bool
+ loadTrainingFeatures(const std::string& file_name,
+ const std::string& labels_file_name)
+ {
+ typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
+ if (pcl::io::loadPCDFile(file_name, *cloud) != 0)
+ return false;
+ std::vector<std::string> labels;
+ std::ifstream f(labels_file_name.c_str());
+ std::string label;
+ while (getline(f, label))
+ if (!label.empty())
+ labels.push_back(label);
+ if (labels.size() != cloud->points.size())
+ return false;
+ setTrainingFeatures(cloud);
+ setTrainingLabels(labels);
+ return true;
+ }
- /** \brief Load the list of training examples and corresponding labels.
- * \param file_name PCD file containing the training features
- * \param labels_file_name the class label for each training example
- * \return true on success, false on failure (read error or number of entries don't match)
- */
- bool
- loadTrainingFeatures(const std::string& file_name, const std::string& labels_file_name)
- {
- typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
- if (pcl::io::loadPCDFile (file_name, *cloud) != 0)
- return (false);
- std::vector<std::string> labels;
- std::ifstream f (labels_file_name.c_str ());
- std::string label;
- while (getline (f, label))
- if (!label.empty ())
- labels.push_back(label);
- if (labels.size () != cloud->points.size ())
- return (false);
- setTrainingFeatures (cloud);
- setTrainingLabels (labels);
- return (true);
+ /**
+ * \brief Save the list of training examples and corresponding labels.
+ * \param file_name file name for writing the training features
+ * \param labels_file_name file name for writing the class label for each
+ * training example
+ * \return true on success, false on failure (write error or number of entries
+ * don't match)
+ */
+ bool
+ saveTrainingFeatures(const std::string& file_name,
+ const std::string& labels_file_name)
+ {
+ typename pcl::PointCloud<PointT>::ConstPtr training_features =
+ tree_->getInputCloud();
+ if (labels_idx_.size() == training_features->points.size()) {
+ if (pcl::io::savePCDFile(file_name.c_str(), *training_features) != 0)
+ return false;
+ std::ofstream f(labels_file_name.c_str());
+ for (const int& i : labels_idx_) {
+ f << classes_[i] << "\n";
}
+ return true;
+ }
+ return false;
+ }
- /** \brief Save the list of training examples and corresponding labels.
- * \param file_name file name for writing the training features
- * \param labels_file_name file name for writing the class label for each training example
- * \return true on success, false on failure (write error or number of entries don't match)
- */
- bool
- saveTrainingFeatures (const std::string& file_name, const std::string& labels_file_name)
- {
- typename pcl::PointCloud<PointT>::ConstPtr training_features = tree_->getInputCloud ();
- if (labels_idx_.size () == training_features->points.size ())
- {
- if (pcl::io::savePCDFile (file_name.c_str (), *training_features) != 0)
- return (false);
- std::ofstream f (labels_file_name.c_str ());
- for (const int& i : labels_idx_)
- {
- f << classes_[i] << "\n";
- }
- return (true);
- }
- return (false);
- }
+ /**
+ * \brief Utility function for the default classification process.
+ * \param p_q the given query point
+ * \param radius the radius of the sphere bounding all of p_q's neighbors
+ * \param gaussian_param influences the width of the Gaussian by specifying where
+ * the 36.78 score should be: score = exp(-distance/gaussian_param)
+ * \param max_nn if given, bounds the maximum returned neighbors to this value
+ * \return pair of label and score for each training class from the neighborhood
+ */
+ ResultPtr
+ classify(const PointT& p_q, double radius, float gaussian_param, int max_nn = INT_MAX)
+ {
+ std::vector<int> k_indices;
+ std::vector<float> k_sqr_distances;
+ getSimilarExemplars(p_q, radius, k_indices, k_sqr_distances, max_nn);
+ return getGaussianBestScores(gaussian_param, k_indices, k_sqr_distances);
+ }
- /** \brief Utility function for the default classification process.
- * \param p_q the given query point
- * \param radius the radius of the sphere bounding all of p_q's neighbors
- * \param gaussian_param influences the width of the Gaussian by specifying where the 36.78 score should be: score = exp(-distance/gaussian_param)
- * \param max_nn if given, bounds the maximum returned neighbors to this value
- * \return pair of label and score for each training class from the neighborhood
- */
- ResultPtr
- classify (const PointT &p_q, double radius, float gaussian_param, int max_nn = INT_MAX)
- {
- std::vector<int> k_indices;
- std::vector<float> k_sqr_distances;
- getSimilarExemplars (p_q, radius, k_indices, k_sqr_distances, max_nn);
- return (getGaussianBestScores (gaussian_param, k_indices, k_sqr_distances));
- }
+ /**
+ * \brief Search for k-nearest neighbors for the given query point.
+ * \param p_q the given query point
+ * \param k the number of neighbors to search for
+ * \param k_indices the resultant indices of the neighboring points
+ * (does not have to be resized to \a k a priori!)
+ * \param k_sqr_distances the resultant squared distances to the neighboring points
+ * (does not have to be resized to \a k a priori!)
+ * \return number of neighbors found
+ */
+ int
+ getKNearestExemplars(const PointT& p_q,
+ int k,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances)
+ {
+ k_indices.resize(k);
+ k_sqr_distances.resize(k);
+ return tree_->nearestKSearch(p_q, k, k_indices, k_sqr_distances);
+ }
- /** \brief Search for k-nearest neighbors for the given query point.
- * \param p_q the given query point
- * \param k the number of neighbors to search for
- * \param k_indices the resultant indices of the neighboring points (does not have to be resized to \a k a priori!)
- * \param k_sqr_distances the resultant squared distances to the neighboring points (does not have be resized to \a k
- * a priori!)
- * \return number of neighbors found
- */
- int
- getKNearestExemplars (const PointT &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances)
- {
- k_indices.resize (k);
- k_sqr_distances.resize (k);
- return (tree_->nearestKSearch (p_q, k, k_indices, k_sqr_distances));
- }
+ /**
+ * \brief Search for all the nearest neighbors of the query point in a given radius.
+ * \param p_q the given query point
+ * \param radius the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices the resultant indices of the neighboring points
+ * \param k_sqr_distances the resultant squared distances to the neighboring points
+ * \param max_nn if given, bounds the maximum returned neighbors to this value
+ * \return number of neighbors found in radius
+ */
+ int
+ getSimilarExemplars(const PointT& p_q,
+ double radius,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ int max_nn = INT_MAX)
+ {
+ return tree_->radiusSearch(p_q, radius, k_indices, k_sqr_distances, max_nn);
+ }
- /** \brief Search for all the nearest neighbors of the query point in a given radius.
- * \param p_q the given query point
- * \param radius the radius of the sphere bounding all of p_q's neighbors
- * \param k_indices the resultant indices of the neighboring points
- * \param k_sqr_distances the resultant squared distances to the neighboring points
- * \param max_nn if given, bounds the maximum returned neighbors to this value
- * \return number of neighbors found in radius
- */
- int
- getSimilarExemplars (const PointT &p_q, double radius, std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances, int max_nn = INT_MAX)
- {
- return (tree_->radiusSearch (p_q, radius, k_indices, k_sqr_distances, max_nn));
- }
+ /**
+ * \brief Gets the smallest square distance to each class given a neighborhood.
+ * \param k_indices the resultant indices of the neighboring points
+ * \param k_sqr_distances the resultant squared distances to the neighboring points
+ * \return a square distance to each training class
+ */
+ std::shared_ptr<std::vector<float>>
+ getSmallestSquaredDistances(std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances)
+ {
+ // Reserve space for distances
+ auto sqr_distances = std::make_shared<std::vector<float>>(classes_.size(), FLT_MAX);
+
+ // Select square distance to each class
+ for (std::vector<int>::const_iterator i = k_indices.begin(); i != k_indices.end();
+ ++i)
+ if ((*sqr_distances)[labels_idx_[*i]] > k_sqr_distances[i - k_indices.begin()])
+ (*sqr_distances)[labels_idx_[*i]] = k_sqr_distances[i - k_indices.begin()];
+ return sqr_distances;
+ }
- /** \brief Gets the smallest square distance to each class given a neighborhood.
- * \param k_indices the resultant indices of the neighboring points
- * \param k_sqr_distances the resultant squared distances to the neighboring points
- * \return a square distance to each training class
- */
- std::shared_ptr<std::vector<float>>
- getSmallestSquaredDistances (std::vector<int> &k_indices, std::vector<float> &k_sqr_distances)
- {
- // Reserve space for distances
- auto sqr_distances = std::make_shared<std::vector<float>> (classes_.size (), FLT_MAX);
+ /**
+ * \brief Computes a score that is inversely proportional to the distance to each
+ * class given a neighborhood.
+ * \note Scores will sum up to one.
+ * \param k_indices the resultant indices of the neighboring points
+ * \param k_sqr_distances the resultant squared distances to the neighboring points
+ * \return pair of label and score for each training class from the neighborhood
+ */
+ ResultPtr
+ getLinearBestScores(std::vector<int>& k_indices, std::vector<float>& k_sqr_distances)
+ {
+ // Get smallest squared distances and transform them to a score for each class
+ auto sqr_distances = getSmallestSquaredDistances(k_indices, k_sqr_distances);
- // Select square distance to each class
- for (std::vector<int>::const_iterator i = k_indices.begin (); i != k_indices.end (); ++i)
- if ((*sqr_distances)[labels_idx_[*i]] > k_sqr_distances[i - k_indices.begin ()])
- (*sqr_distances)[labels_idx_[*i]] = k_sqr_distances[i - k_indices.begin ()];
- return (sqr_distances);
+ // Transform distances to scores
+ double sum_dist = 0;
+ auto result =
+ std::make_shared<std::pair<std::vector<std::string>, std::vector<float>>>();
+ result->first.reserve(classes_.size());
+ result->second.reserve(classes_.size());
+ for (std::vector<float>::const_iterator it = sqr_distances->begin();
+ it != sqr_distances->end();
+ ++it)
+ if (*it != FLT_MAX) {
+ result->first.push_back(classes_[it - sqr_distances->begin()]);
+ result->second.push_back(sqrt(*it));
+ sum_dist += result->second.back();
}
+ for (float& it : result->second)
+ it = 1 - it / sum_dist;
- /** \brief Computes a score that is inversely proportional to the distance to each class given a neighborhood.
- * \note Scores will sum up to one.
- * \param k_indices the resultant indices of the neighboring points
- * \param k_sqr_distances the resultant squared distances to the neighboring points
- * \return pair of label and score for each training class from the neighborhood
- */
- ResultPtr
- getLinearBestScores (std::vector<int> &k_indices, std::vector<float> &k_sqr_distances)
- {
- // Get smallest squared distances and transform them to a score for each class
- auto sqr_distances = getSmallestSquaredDistances (k_indices, k_sqr_distances);
+ // Return label/score list pair
+ return result;
+ }
- // Transform distances to scores
- double sum_dist = 0;
- auto result = std::make_shared<std::pair<std::vector<std::string>, std::vector<float>>> ();
- result->first.reserve (classes_.size ());
- result->second.reserve (classes_.size ());
- for (std::vector<float>::const_iterator it = sqr_distances->begin (); it != sqr_distances->end (); ++it)
- if (*it != FLT_MAX)
- {
- result->first.push_back (classes_[it - sqr_distances->begin ()]);
- result->second.push_back (sqrt (*it));
- sum_dist += result->second.back ();
- }
- for (float &it : result->second)
- it = 1 - it/sum_dist;
+ /**
+ * \brief Computes a score exponentially decreasing with the distance for each class
+ * given a neighborhood.
+ * \param[in] gaussian_param influences the width of the Gaussian:
+ * score = exp(-distance/gaussioan_param)
+ * \param[out] k_indices the resultant indices of the neighboring points
+ * \param[out] k_sqr_distances the resultant squared distances to the
+ * neighboring points
+ * \return pair of label and score for each training class from the neighborhood
+ */
+ ResultPtr
+ getGaussianBestScores(float gaussian_param,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances)
+ {
+ // Get smallest squared distances and transform them to a score for each class
+ auto sqr_distances = getSmallestSquaredDistances(k_indices, k_sqr_distances);
- // Return label/score list pair
- return (result);
+ // Transform distances to scores
+ auto result =
+ std::make_shared<std::pair<std::vector<std::string>, std::vector<float>>>();
+ result->first.reserve(classes_.size());
+ result->second.reserve(classes_.size());
+ for (std::vector<float>::const_iterator it = sqr_distances->begin();
+ it != sqr_distances->end();
+ ++it)
+ if (*it != FLT_MAX) {
+ result->first.push_back(classes_[it - sqr_distances->begin()]);
+ // TODO leave it squared, and relate param to sigma...
+ result->second.push_back(std::exp(-std::sqrt(*it) / gaussian_param));
}
- /** \brief Computes a score exponentially decreasing with the distance for each class given a neighborhood.
- * \param[in] gaussian_param influences the width of the Gaussian: score = exp(-distance/gaussioan_param)
- * \param[out] k_indices the resultant indices of the neighboring points
- * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
- * \return pair of label and score for each training class from the neighborhood
- */
- ResultPtr
- getGaussianBestScores (float gaussian_param, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances)
- {
- // Get smallest squared distances and transform them to a score for each class
- auto sqr_distances = getSmallestSquaredDistances (k_indices, k_sqr_distances);
+ // Return label/score list pair
+ return result;
+ }
+};
- // Transform distances to scores
- auto result = std::make_shared<std::pair<std::vector<std::string>, std::vector<float>>> ();
- result->first.reserve (classes_.size ());
- result->second.reserve (classes_.size ());
- for (std::vector<float>::const_iterator it = sqr_distances->begin (); it != sqr_distances->end (); ++it)
- if (*it != FLT_MAX)
- {
- result->first.push_back (classes_[it - sqr_distances->begin ()]);
- // TODO leave it squared, and relate param to sigma...
- result->second.push_back (std::exp (-std::sqrt (*it) / gaussian_param));
- }
-
- // Return label/score list pair
- return (result);
- }
- };
-}
+} // namespace pcl
#pragma once
-// PCL
#include <pcl/apps/openni_passthrough_qt.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
#include <pcl/common/time.h>
-#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
// Useful macros
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
-namespace Ui
-{
- class MainWindow;
+namespace Ui {
+class MainWindow;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-class OpenNIPassthrough : public QMainWindow
-{
+class OpenNIPassthrough : public QMainWindow {
Q_OBJECT
- public:
- using Cloud = pcl::PointCloud<pcl::PointXYZRGBA>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
+public:
+ using Cloud = pcl::PointCloud<pcl::PointXYZRGBA>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ OpenNIPassthrough(pcl::OpenNIGrabber& grabber);
+
+ ~OpenNIPassthrough()
+ {
+ if (grabber_.isRunning())
+ grabber_.stop();
+ }
+
+ void
+ cloud_cb(const CloudConstPtr& cloud);
- OpenNIPassthrough (pcl::OpenNIGrabber& grabber);
+protected:
+ pcl::visualization::PCLVisualizer::Ptr vis_;
+ pcl::OpenNIGrabber& grabber_;
+ std::string device_id_;
+ CloudPtr cloud_pass_;
+ pcl::PassThrough<pcl::PointXYZRGBA> pass_;
- ~OpenNIPassthrough ()
- {
- if (grabber_.isRunning ())
- grabber_.stop ();
- }
-
- void
- cloud_cb (const CloudConstPtr& cloud);
+private:
+ QMutex mtx_;
+ Ui::MainWindow* ui_;
+ QTimer* vis_timer_;
- protected:
- pcl::visualization::PCLVisualizer::Ptr vis_;
- pcl::OpenNIGrabber& grabber_;
- std::string device_id_;
- CloudPtr cloud_pass_;
- pcl::PassThrough<pcl::PointXYZRGBA> pass_;
+public Q_SLOTS:
+ void
+ adjustPassThroughValues(int new_value)
+ {
+ pass_.setFilterLimits(0.0f, float(new_value) / 10.0f);
+ PCL_INFO("Changed passthrough maximum value to: %f\n", float(new_value) / 10.0f);
+ }
- private:
- QMutex mtx_;
- Ui::MainWindow *ui_;
- QTimer *vis_timer_;
+private Q_SLOTS:
+ void
+ timeoutSlot();
- public Q_SLOTS:
- void
- adjustPassThroughValues (int new_value)
- {
- pass_.setFilterLimits (0.0f, float (new_value) / 10.0f);
- PCL_INFO ("Changed passthrough maximum value to: %f\n", float (new_value) / 10.0f);
- }
-
- private Q_SLOTS:
- void
- timeoutSlot ();
-
- Q_SIGNALS:
- void
- valueChanged (int new_value);
+Q_SIGNALS:
+ void
+ valueChanged(int new_value);
};
#pragma GCC system_header
#endif
-#include <ui_openni_passthrough.h>
-// QT4
#include <QMainWindow>
#include <QMutex>
-#include <QTimer>
#include <QObject>
+#include <QTimer>
+#include <ui_openni_passthrough.h>
#pragma once
#include <pcl/apps/organized_segmentation_demo_qt.h>
-
-// PCL
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
+#include <pcl/common/time.h>
+#include <pcl/common/transforms.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/filters/voxel_grid.h>
#include <pcl/io/oni_grabber.h>
+#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/common/time.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/features/integral_image_normal.h>
+#include <pcl/segmentation/edge_aware_plane_comparator.h>
+#include <pcl/segmentation/euclidean_cluster_comparator.h>
+#include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
+#include <pcl/segmentation/organized_connected_component_segmentation.h>
#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/planar_polygon_fusion.h>
-#include <pcl/common/transforms.h>
#include <pcl/segmentation/plane_coefficient_comparator.h>
-#include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
#include <pcl/segmentation/rgb_plane_coefficient_comparator.h>
-#include <pcl/segmentation/edge_aware_plane_comparator.h>
-#include <pcl/segmentation/euclidean_cluster_comparator.h>
-#include <pcl/segmentation/organized_connected_component_segmentation.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
using PointT = pcl::PointXYZRGBA;
// Useful macros
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
-
-namespace Ui
-{
- class MainWindow;
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
+
+namespace Ui {
+class MainWindow;
}
-class OrganizedSegmentationDemo : public QMainWindow
-{
+class OrganizedSegmentationDemo : public QMainWindow {
Q_OBJECT
- public:
- using Cloud = pcl::PointCloud<PointT>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
-
-
- OrganizedSegmentationDemo(pcl::Grabber& grabber);
-
- ~OrganizedSegmentationDemo ()
- {
- if(grabber_.isRunning())
- grabber_.stop();
- }
-
- void cloud_cb (const CloudConstPtr& cloud);
-
- protected:
- pcl::visualization::PCLVisualizer::Ptr vis_;
- pcl::Grabber& grabber_;
-
- QMutex mtx_;
- QMutex vis_mtx_;
- Ui::MainWindow *ui_;
- QTimer *vis_timer_;
- pcl::PointCloud<PointT> prev_cloud_;
- pcl::PointCloud<pcl::Normal> prev_normals_;
- std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > prev_regions_;
- float* prev_distance_map_;
-
- pcl::PointCloud<PointT>::CloudVectorType prev_clusters_;
-
- pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
- pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
-
- bool capture_;
- bool data_modified_;
- std::size_t previous_data_size_;
- std::size_t previous_clusters_size_;
-
- bool display_normals_;
- bool display_curvature_;
- bool display_distance_map_;
-
- bool use_planar_refinement_;
- bool use_clustering_;
-
- pcl::PlaneCoefficientComparator<PointT, pcl::Normal>::Ptr plane_comparator_;
- pcl::EuclideanPlaneCoefficientComparator<PointT, pcl::Normal>::Ptr euclidean_comparator_;
- pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal>::Ptr rgb_comparator_;
- pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal> rgb_comp_;
- pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>::Ptr edge_aware_comparator_;
- pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr euclidean_cluster_comparator_;
-
- public Q_SLOTS:
- void toggleCapturePressed()
- {
- capture_ = !capture_;
- }
-
- void usePlaneComparatorPressed ();
- void useEuclideanComparatorPressed ();
- void useRGBComparatorPressed ();
- void useEdgeAwareComparatorPressed ();
-
- void displayCurvaturePressed ();
- void displayDistanceMapPressed ();
- void displayNormalsPressed ();
-
- void disableRefinementPressed ()
- {
- use_planar_refinement_ = false;
- }
-
- void usePlanarRefinementPressed ()
- {
- use_planar_refinement_ = true;
- }
-
- void disableClusteringPressed ()
- {
- use_clustering_ = false;
- }
-
- void useEuclideanClusteringPressed ()
- {
- use_clustering_ = true;
- }
-
-
- private Q_SLOTS:
+public:
+ using Cloud = pcl::PointCloud<PointT>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ OrganizedSegmentationDemo(pcl::Grabber& grabber);
+
+ ~OrganizedSegmentationDemo()
+ {
+ if (grabber_.isRunning())
+ grabber_.stop();
+ }
+
+ void
+ cloud_cb(const CloudConstPtr& cloud);
+
+protected:
+ pcl::visualization::PCLVisualizer::Ptr vis_;
+ pcl::Grabber& grabber_;
+
+ QMutex mtx_;
+ QMutex vis_mtx_;
+ Ui::MainWindow* ui_;
+ QTimer* vis_timer_;
+ pcl::PointCloud<PointT> prev_cloud_;
+ pcl::PointCloud<pcl::Normal> prev_normals_;
+ std::vector<pcl::PlanarRegion<PointT>,
+ Eigen::aligned_allocator<pcl::PlanarRegion<PointT>>>
+ prev_regions_;
+ float* prev_distance_map_;
+
+ pcl::PointCloud<PointT>::CloudVectorType prev_clusters_;
+
+ pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+ pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
+
+ bool capture_;
+ bool data_modified_;
+ std::size_t previous_data_size_;
+ std::size_t previous_clusters_size_;
+
+ bool display_normals_;
+ bool display_curvature_;
+ bool display_distance_map_;
+
+ bool use_planar_refinement_;
+ bool use_clustering_;
+
+ pcl::PlaneCoefficientComparator<PointT, pcl::Normal>::Ptr plane_comparator_;
+ pcl::EuclideanPlaneCoefficientComparator<PointT, pcl::Normal>::Ptr
+ euclidean_comparator_;
+ pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal>::Ptr rgb_comparator_;
+ pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal> rgb_comp_;
+ pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>::Ptr edge_aware_comparator_;
+ pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr
+ euclidean_cluster_comparator_;
+
+public Q_SLOTS:
+ void
+ toggleCapturePressed()
+ {
+ capture_ = !capture_;
+ }
+
+ void
+ usePlaneComparatorPressed();
+ void
+ useEuclideanComparatorPressed();
void
- timeoutSlot();
+ useRGBComparatorPressed();
+ void
+ useEdgeAwareComparatorPressed();
+ void
+ displayCurvaturePressed();
+ void
+ displayDistanceMapPressed();
+ void
+ displayNormalsPressed();
+
+ void
+ disableRefinementPressed()
+ {
+ use_planar_refinement_ = false;
+ }
+
+ void
+ usePlanarRefinementPressed()
+ {
+ use_planar_refinement_ = true;
+ }
+
+ void
+ disableClusteringPressed()
+ {
+ use_clustering_ = false;
+ }
+
+ void
+ useEuclideanClusteringPressed()
+ {
+ use_clustering_ = true;
+ }
+
+private Q_SLOTS:
+ void
+ timeoutSlot();
};
/*
* Software License Agreement (BSD License)
- *
+ *
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
- *
+ *
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
- * are met:
- *
+ * are met:
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#pragma GCC system_header
#endif
-#include <ui_organized_segmentation_demo.h>
-// QT4
#include <QMainWindow>
#include <QMutex>
#include <QTimer>
+#include <ui_organized_segmentation_demo.h>
/*
* Software License Agreement (BSD License)
- *
+ *
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
- *
+ *
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
- * are met:
- *
+ * are met:
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include <ui_pcd_video_player.h>
-
-#include <iostream>
-#include <ctime>
-
-// QT4
-#include <QMainWindow>
-#include <QMutex>
-#include <QTimer>
-
-// Boost
-#include <boost/filesystem.hpp>
-
-// PCL
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
-
-#include <pcl/common/common.h>
#include <pcl/common/angles.h>
+#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <pcl/common/transforms.h>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/io/pcd_io.h>
-
+#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
-#include <pcl/registration/transformation_estimation_svd.h>
+#include <boost/filesystem.hpp>
+
+#include <QMainWindow>
+#include <QMutex>
+#include <QTimer>
+#include <ui_pcd_video_player.h>
+
+#include <ctime>
+#include <iostream>
#define CURRENT_VERSION 0.2
// Useful macros
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
-
-namespace Ui
-{
- class MainWindow;
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
+
+namespace Ui {
+class MainWindow;
}
-class PCDVideoPlayer : public QMainWindow
-{
+class PCDVideoPlayer : public QMainWindow {
Q_OBJECT
- public:
- using Cloud = pcl::PointCloud<pcl::PointXYZRGBA>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
-
- PCDVideoPlayer ();
-
- ~PCDVideoPlayer () {}
-
- protected:
- pcl::visualization::PCLVisualizer::Ptr vis_;
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_;
-
- QMutex mtx_;
- QMutex vis_mtx_;
- Ui::MainWindow *ui_;
- QTimer *vis_timer_;
-
- QString dir_;
-
- std::vector<std::string> pcd_files_;
- std::vector<boost::filesystem::path> pcd_paths_;
-
-
- /** \brief The current displayed frame */
- unsigned int current_frame_;
- /** \brief Store the number of loaded frames */
- unsigned int nr_of_frames_;
-
- /** \brief Indicate that pointclouds were loaded */
- bool cloud_present_;
- /** \brief Indicate that the timeoutSlot needs to reload the pointcloud */
- bool cloud_modified_;
-
- /** \brief Indicate that files should play continuously */
- bool play_mode_;
- /** \brief In play mode only update if speed_counter_ == speed_value */
- unsigned int speed_counter_;
- /** \brief Fixes the speed in steps of 5ms, default 5, gives 5+1 * 5ms = 30ms = 33,3 Hz playback speed */
- unsigned int speed_value_;
-
- public Q_SLOTS:
- void
- playButtonPressed ()
- { play_mode_ = true; }
-
- void
- stopButtonPressed()
- { play_mode_ = false; }
-
- void
- backButtonPressed ();
-
- void
- nextButtonPressed ();
-
- void
- selectFolderButtonPressed ();
-
- void
- selectFilesButtonPressed ();
-
- void
- indexSliderValueChanged (int value);
-
- private Q_SLOTS:
- void
- timeoutSlot ();
-
+public:
+ using Cloud = pcl::PointCloud<pcl::PointXYZRGBA>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ PCDVideoPlayer();
+
+ ~PCDVideoPlayer() {}
+
+protected:
+ pcl::visualization::PCLVisualizer::Ptr vis_;
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_;
+
+ QMutex mtx_;
+ QMutex vis_mtx_;
+ Ui::MainWindow* ui_;
+ QTimer* vis_timer_;
+
+ QString dir_;
+
+ std::vector<std::string> pcd_files_;
+ std::vector<boost::filesystem::path> pcd_paths_;
+
+ /** \brief The current displayed frame */
+ unsigned int current_frame_;
+ /** \brief Store the number of loaded frames */
+ unsigned int nr_of_frames_;
+
+ /** \brief Indicate that pointclouds were loaded */
+ bool cloud_present_;
+ /** \brief Indicate that the timeoutSlot needs to reload the pointcloud */
+ bool cloud_modified_;
+
+ /** \brief Indicate that files should play continuously */
+ bool play_mode_;
+ /** \brief In play mode only update if speed_counter_ == speed_value */
+ unsigned int speed_counter_;
+ /**
+ * \brief Fixes the speed in steps of 5ms, default 5, gives 5+1 * 5ms = 30ms = 33,3
+ * Hz playback speed
+ */
+ unsigned int speed_value_;
+
+public Q_SLOTS:
+ void
+ playButtonPressed()
+ {
+ play_mode_ = true;
+ }
+
+ void
+ stopButtonPressed()
+ {
+ play_mode_ = false;
+ }
+
+ void
+ backButtonPressed();
+
+ void
+ nextButtonPressed();
+
+ void
+ selectFolderButtonPressed();
+
+ void
+ selectFilesButtonPressed();
+
+ void
+ indexSliderValueChanged(int value);
+
+private Q_SLOTS:
+ void
+ timeoutSlot();
};
#include <pcl/common/common.h>
-#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
+#include <vtkSmartPointer.h>
#include <functional>
-namespace pcl
-{
- namespace apps
- {
- /** \brief @b Class to render synthetic views of a 3D mesh using a tessellated sphere
- * NOTE: This class should replace renderViewTesselatedSphere from pcl::visualization.
- * Some extensions are planned in the near future to this class like removal of duplicated views for
- * symmetrical objects, generation of RGB synthetic clouds when RGB available on mesh, etc.
- * \author Aitor Aldoma
- * \ingroup apps
- */
- class PCL_EXPORTS RenderViewsTesselatedSphere
+namespace pcl {
+namespace apps {
+
+/**
+ * \brief @b Class to render synthetic views of a 3D mesh using a tessellated sphere
+ * NOTE: This class should replace renderViewTesselatedSphere from pcl::visualization.
+ * Some extensions are planned in the near future to this class like removal of
+ * duplicated views for symmetrical objects, generation of RGB synthetic clouds when RGB
+ * available on mesh, etc.
+ * \author Aitor Aldoma
+ * \ingroup apps
+ */
+class PCL_EXPORTS RenderViewsTesselatedSphere {
+private:
+ std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> poses_;
+ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> generated_views_;
+ std::vector<float> entropies_;
+ int resolution_;
+ int tesselation_level_;
+ bool use_vertices_;
+ float view_angle_;
+ float radius_sphere_;
+ bool compute_entropy_;
+ vtkSmartPointer<vtkPolyData> polydata_;
+ bool gen_organized_;
+ std::function<bool(const Eigen::Vector3f&)> campos_constraints_func_;
+
+ struct camPosConstraintsAllTrue {
+ bool
+ operator()(const Eigen::Vector3f& /*pos*/) const
{
- private:
- std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > poses_;
- std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> generated_views_;
- std::vector<float> entropies_;
- int resolution_;
- int tesselation_level_;
- bool use_vertices_;
- float view_angle_;
- float radius_sphere_;
- bool compute_entropy_;
- vtkSmartPointer<vtkPolyData> polydata_;
- bool gen_organized_;
- std::function<bool
- (const Eigen::Vector3f &)> campos_constraints_func_;
-
- struct camPosConstraintsAllTrue
- {
- bool
- operator() (const Eigen::Vector3f & /*pos*/) const
- {
- return true;
- }
- ;
- };
-
- public:
- RenderViewsTesselatedSphere ()
- {
- resolution_ = 150;
- tesselation_level_ = 1;
- use_vertices_ = false;
- view_angle_ = 57;
- radius_sphere_ = 1.f;
- compute_entropy_ = false;
- gen_organized_ = false;
- campos_constraints_func_ = camPosConstraintsAllTrue ();
- }
-
- void
- setCamPosConstraints (std::function<bool (const Eigen::Vector3f &)> & bb)
- {
- campos_constraints_func_ = bb;
- }
-
- /* \brief Indicates whether to generate organized or unorganized data
- * \param b organized/unorganized
- */
- void
- setGenOrganized (bool b)
- {
- gen_organized_ = b;
- }
-
- /* \brief Sets the size of the render window
- * \param res resolution size
- */
- void
- setResolution (int res)
- {
- resolution_ = res;
- }
-
- /* \brief Whether to use the vertices or triangle centers of the tessellated sphere
- * \param use true indicates to use vertices, false triangle centers
- */
-
- void
- setUseVertices (bool use)
- {
- use_vertices_ = use;
- }
-
- /* \brief Radius of the sphere where the virtual camera will be placed
- * \param use true indicates to use vertices, false triangle centers
- */
- void
- setRadiusSphere (float radius)
- {
- radius_sphere_ = radius;
- }
-
- /* \brief Whether to compute the entropies (level of occlusions for each view)
- * \param compute true to compute entropies, false otherwise
- */
- void
- setComputeEntropies (bool compute)
- {
- compute_entropy_ = compute;
- }
-
- /* \brief How many times the icosahedron should be tessellated. Results in more or less camera positions and generated views.
- * \param level amount of tessellation
- */
- void
- setTesselationLevel (int level)
- {
- tesselation_level_ = level;
- }
-
- /* \brief Sets the view angle of the virtual camera
- * \param angle view angle in degrees
- */
- void
- setViewAngle (float angle)
- {
- view_angle_ = angle;
- }
-
- /* \brief adds the mesh to be used as a vtkPolyData
- * \param polydata vtkPolyData object
- */
- void
- addModelFromPolyData (vtkSmartPointer<vtkPolyData> &polydata)
- {
- polydata_ = polydata;
- }
-
- /* \brief performs the rendering and stores the generated information
- */
- void
- generateViews ();
-
- /* \brief Get the generated poses for the generated views
- * \param poses 4x4 matrices representing the pose of the cloud relative to the model coordinate system
- */
- void
- getPoses (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & poses)
- {
- poses = poses_;
- }
-
- /* \brief Get the generated views
- * \param views generated pointclouds in camera coordinates
- */
- void
- getViews (std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> & views)
- {
- views = generated_views_;
- }
-
- /* \brief Get the entropies (level of occlusions) for the views
- * \param entropies level of occlusions
- */
- void
- getEntropies (std::vector<float> & entropies)
- {
- entropies = entropies_;
- }
+ return true;
};
+ };
+
+public:
+ RenderViewsTesselatedSphere()
+ {
+ resolution_ = 150;
+ tesselation_level_ = 1;
+ use_vertices_ = false;
+ view_angle_ = 57;
+ radius_sphere_ = 1.f;
+ compute_entropy_ = false;
+ gen_organized_ = false;
+ campos_constraints_func_ = camPosConstraintsAllTrue();
+ }
+
+ void
+ setCamPosConstraints(std::function<bool(const Eigen::Vector3f&)>& bb)
+ {
+ campos_constraints_func_ = bb;
+ }
+
+ /**
+ * \brief Indicates whether to generate organized or unorganized data
+ * \param b organized/unorganized
+ */
+ void
+ setGenOrganized(bool b)
+ {
+ gen_organized_ = b;
+ }
+ /**
+ * \brief Sets the size of the render window
+ * \param res resolution size
+ */
+ void
+ setResolution(int res)
+ {
+ resolution_ = res;
}
-}
+
+ /**
+ * \brief Whether to use the vertices or triangle centers of the tessellated sphere
+ * \param use true indicates to use vertices, false triangle centers
+ */
+
+ void
+ setUseVertices(bool use)
+ {
+ use_vertices_ = use;
+ }
+
+ /**
+ * \brief Radius of the sphere where the virtual camera will be placed
+ * \param use true indicates to use vertices, false triangle centers
+ */
+ void
+ setRadiusSphere(float radius)
+ {
+ radius_sphere_ = radius;
+ }
+
+ /**
+ * \brief Whether to compute the entropies (level of occlusions for each view)
+ * \param compute true to compute entropies, false otherwise
+ */
+ void
+ setComputeEntropies(bool compute)
+ {
+ compute_entropy_ = compute;
+ }
+
+ /**
+ * \brief How many times the icosahedron should be tessellated. Results in more or
+ * less camera positions and generated views.
+ * \param level amount of tessellation
+ */
+ void
+ setTesselationLevel(int level)
+ {
+ tesselation_level_ = level;
+ }
+
+ /**
+ * \brief Sets the view angle of the virtual camera
+ * \param angle view angle in degrees
+ */
+ void
+ setViewAngle(float angle)
+ {
+ view_angle_ = angle;
+ }
+
+ /**
+ * \brief adds the mesh to be used as a vtkPolyData
+ * \param polydata vtkPolyData object
+ */
+ void
+ addModelFromPolyData(vtkSmartPointer<vtkPolyData>& polydata)
+ {
+ polydata_ = polydata;
+ }
+
+ /**
+ * \brief performs the rendering and stores the generated information
+ */
+ void
+ generateViews();
+
+ /**
+ * \brief Get the generated poses for the generated views
+ * \param poses 4x4 matrices representing the pose of the cloud relative to the model
+ * coordinate system
+ */
+ void
+ getPoses(
+ std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>& poses)
+ {
+ poses = poses_;
+ }
+
+ /**
+ * \brief Get the generated views
+ * \param views generated pointclouds in camera coordinates
+ */
+ void
+ getViews(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& views)
+ {
+ views = generated_views_;
+ }
+
+ /**
+ * \brief Get the entropies (level of occlusions) for the views
+ * \param entropies level of occlusions
+ */
+ void
+ getEntropies(std::vector<float>& entropies)
+ {
+ entropies = entropies_;
+ }
+};
+
+} // namespace apps
+} // namespace pcl
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012, Open Perception, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
#include <pcl/common/time.h> //fps calculations
#if SHOW_FPS
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
#else
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
-}while(false)
+#define FPS_CALC(_WHAT_) \
+ do { \
+ } while (false)
#endif
#pragma once
-#include <fstream>
-#include <pcl/point_types.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/features/vfh.h>
-#include <pcl/features/normal_3d.h>
#include <pcl/apps/nn_classification.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/vfh.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/PCLPointCloud2.h>
+#include <pcl/point_types.h>
-namespace pcl
-{
- /** \brief Helper function to extract the VFH feature describing the given point cloud.
- * \param points point cloud for feature extraction
- * \param radius search radius for normal estimation
- * \return point cloud containing the extracted feature
- */
- template <typename PointT> pcl::PointCloud<pcl::VFHSignature308>::Ptr
- computeVFH (typename PointCloud<PointT>::ConstPtr cloud, double radius)
- {
- using namespace pcl;
-
- // Create an empty kdtree representation, and pass it to the objects.
- // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
- typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
+#include <fstream>
- // Create the normal estimation class, and pass the input dataset to it
- NormalEstimation<PointT, Normal> ne;
- ne.setInputCloud (cloud);
- ne.setSearchMethod (tree);
+namespace pcl {
- // Use all neighbors in a sphere of given radius to compute the normals
- PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
- ne.setRadiusSearch (radius);
- ne.compute (*normals);
+/**
+ * \brief Helper function to extract the VFH feature describing the given point cloud.
+ * \param points point cloud for feature extraction
+ * \param radius search radius for normal estimation
+ * \return point cloud containing the extracted feature
+ */
+template <typename PointT>
+pcl::PointCloud<pcl::VFHSignature308>::Ptr
+computeVFH(typename PointCloud<PointT>::ConstPtr cloud, double radius)
+{
+ using namespace pcl;
- // Create the VFH estimation class, and pass the input dataset+normals to it
- VFHEstimation<PointT, Normal, VFHSignature308> vfh;
- vfh.setInputCloud (cloud);
- vfh.setInputNormals (normals);
- vfh.setSearchMethod (tree);
+ // Create an empty kdtree representation, and pass it to the objects.
+ // Its content will be filled inside the object, based on the given input dataset
+ // (as no other search surface is given).
+ typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
- // Output datasets
- PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308>);
+ // Create the normal estimation class, and pass the input dataset to it
+ NormalEstimation<PointT, Normal> ne;
+ ne.setInputCloud(cloud);
+ ne.setSearchMethod(tree);
- // Compute the features and return
- vfh.compute (*vfhs);
- return vfhs;
- }
+ // Use all neighbors in a sphere of given radius to compute the normals
+ PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
+ ne.setRadiusSearch(radius);
+ ne.compute(*normals);
- /**
- * \brief Utility class for nearest neighbor search based classification of VFH features.
- * \author Zoltan Csaba Marton
- */
- class VFHClassifierNN
- {
- public:
+ // Create the VFH estimation class, and pass the input dataset+normals to it
+ VFHEstimation<PointT, Normal, VFHSignature308> vfh;
+ vfh.setInputCloud(cloud);
+ vfh.setInputNormals(normals);
+ vfh.setSearchMethod(tree);
- using FeatureCloud = pcl::PointCloud<pcl::VFHSignature308>;
- using FeatureCloudPtr = pcl::PointCloud<pcl::VFHSignature308>::Ptr;
- using FeatureCloudConstPtr = pcl::PointCloud<pcl::VFHSignature308>::ConstPtr;
- using Result = NNClassification<pcl::VFHSignature308>::Result;
- using ResultPtr = NNClassification<pcl::VFHSignature308>::ResultPtr;
+ // Output datasets
+ PointCloud<VFHSignature308>::Ptr vfhs(new PointCloud<VFHSignature308>);
- private:
+ // Compute the features and return
+ vfh.compute(*vfhs);
+ return vfhs;
+}
- /** \brief Point cloud containing the training VFH features */
- FeatureCloudPtr training_features_;
- /** \brief Class label for each training example */
- std::vector<std::string> labels_;
- /** \brief Nearest neighbor classifier instantiated for VFH features */
- NNClassification<pcl::VFHSignature308> classifier_;
+/**
+ * \brief Utility class for nearest neighbor search based classification of VFH
+ * features.
+ * \author Zoltan Csaba Marton
+ */
+class VFHClassifierNN {
+public:
+ using FeatureCloud = pcl::PointCloud<pcl::VFHSignature308>;
+ using FeatureCloudPtr = pcl::PointCloud<pcl::VFHSignature308>::Ptr;
+ using FeatureCloudConstPtr = pcl::PointCloud<pcl::VFHSignature308>::ConstPtr;
+ using Result = NNClassification<pcl::VFHSignature308>::Result;
+ using ResultPtr = NNClassification<pcl::VFHSignature308>::ResultPtr;
- public:
+private:
+ /** \brief Point cloud containing the training VFH features */
+ FeatureCloudPtr training_features_;
+ /** \brief Class label for each training example */
+ std::vector<std::string> labels_;
+ /** \brief Nearest neighbor classifier instantiated for VFH features */
+ NNClassification<pcl::VFHSignature308> classifier_;
- VFHClassifierNN ()
- {
- reset ();
- }
+public:
+ VFHClassifierNN() { reset(); }
- void reset ()
- {
- training_features_.reset (new FeatureCloud);
- labels_.clear ();
- classifier_ = NNClassification<pcl::VFHSignature308> ();
- }
+ void
+ reset()
+ {
+ training_features_.reset(new FeatureCloud);
+ labels_.clear();
+ classifier_ = NNClassification<pcl::VFHSignature308>();
+ }
- /** \brief Set up the classifier with the current training features and labels */
- void finalizeTraining ()
- {
- finalizeTree ();
- finalizeLabels ();
- }
+ /** \brief Set up the classifier with the current training features and labels */
+ void
+ finalizeTraining()
+ {
+ finalizeTree();
+ finalizeLabels();
+ }
- /** \brief Set up the classifier with the current training features */
- void finalizeTree ()
- {
- classifier_.setTrainingFeatures(training_features_);
- }
+ /** \brief Set up the classifier with the current training features */
+ void
+ finalizeTree()
+ {
+ classifier_.setTrainingFeatures(training_features_);
+ }
- /** \brief Set up the classifier with the current training example labels */
- void finalizeLabels ()
- {
- classifier_.setTrainingLabels(labels_);
- }
+ /** \brief Set up the classifier with the current training example labels */
+ void
+ finalizeLabels()
+ {
+ classifier_.setTrainingLabels(labels_);
+ }
- /** \brief Save the list of training examples and corresponding labels.
- * \param file_name file name for writing the training features
- * \param labels_file_name file name for writing the class label for each training example
- * \return true on success, false on failure (write error or number of entries don't match)
- */
- bool saveTrainingFeatures(const std::string& file_name, const std::string& labels_file_name)
- {
- if (labels_.size () == training_features_->points.size ())
- {
- if (pcl::io::savePCDFile (file_name, *training_features_) != 0)
- return false;
- std::ofstream f (labels_file_name.c_str ());
- for (const auto& s : labels_)
- {
- f << s << "\n";
- }
- return true;
- }
+ /**
+ * \brief Save the list of training examples and corresponding labels.
+ * \param file_name file name for writing the training features
+ * \param labels_file_name file name for writing the class label for each
+ * training example
+ * \return true on success, false on failure (write error or number of entries
+ * don't match)
+ */
+ bool
+ saveTrainingFeatures(const std::string& file_name,
+ const std::string& labels_file_name)
+ {
+ if (labels_.size() == training_features_->points.size()) {
+ if (pcl::io::savePCDFile(file_name, *training_features_) != 0)
return false;
+ std::ofstream f(labels_file_name.c_str());
+ for (const auto& s : labels_) {
+ f << s << "\n";
}
+ return true;
+ }
+ return false;
+ }
- /** \brief Fill the list of training examples and corresponding labels.
- * \note this function has a cumulative effect.
- * \param training_features the training features
- * \param labels the class label for each training example
- * \return true on success, false on failure (number of entries don't match)
- */
- bool addTrainingFeatures (const FeatureCloudPtr& training_features, const std::vector<std::string> &labels)
- {
- if (labels.size () == training_features->points.size ())
- {
- labels_.insert (labels_.end (), labels.begin (), labels.end ());
- training_features_->points.insert (training_features_->points.end (), training_features->points.begin (), training_features->points.end ());
- training_features_->header = training_features->header;
- training_features_->height = 1;
- training_features_->width = static_cast<std::uint32_t> (training_features_->points.size ());
- training_features_->is_dense &= training_features->is_dense;
- training_features_->sensor_origin_ = training_features->sensor_origin_;
- training_features_->sensor_orientation_ = training_features->sensor_orientation_;
- return true;
- }
- return false;
- }
+ /**
+ * \brief Fill the list of training examples and corresponding labels.
+ * \note this function has a cumulative effect.
+ * \param training_features the training features
+ * \param labels the class label for each training example
+ * \return true on success, false on failure (number of entries don't match)
+ */
+ bool
+ addTrainingFeatures(const FeatureCloudPtr& training_features,
+ const std::vector<std::string>& labels)
+ {
+ if (labels.size() == training_features->points.size()) {
+ labels_.insert(labels_.end(), labels.begin(), labels.end());
+ training_features_->points.insert(training_features_->points.end(),
+ training_features->points.begin(),
+ training_features->points.end());
+ training_features_->header = training_features->header;
+ training_features_->height = 1;
+ training_features_->width =
+ static_cast<std::uint32_t>(training_features_->points.size());
+ training_features_->is_dense &= training_features->is_dense;
+ training_features_->sensor_origin_ = training_features->sensor_origin_;
+ training_features_->sensor_orientation_ = training_features->sensor_orientation_;
+ return true;
+ }
+ return false;
+ }
- /** \brief Fill the list of training examples and corresponding labels.
- * \note this function has a cumulative effect.
- * \param file_name PCD file containing the training features
- * \param labels_file_name the class label for each training example
- * \return true on success, false on failure (read error or number of entries don't match)
- */
- bool loadTrainingFeatures(const std::string& file_name, const std::string& labels_file_name)
- {
- FeatureCloudPtr cloud (new FeatureCloud);
- if (pcl::io::loadPCDFile (file_name, *cloud) != 0)
- return false;
- std::vector<std::string> labels;
- std::ifstream f (labels_file_name.c_str ());
- std::string label;
- while (getline (f, label))
- if (!label.empty ())
- labels.push_back(label);
- return addTrainingFeatures (cloud, labels);
- }
+ /**
+ * \brief Fill the list of training examples and corresponding labels.
+ * \note this function has a cumulative effect.
+ * \param file_name PCD file containing the training features
+ * \param labels_file_name the class label for each training example
+ * \return true on success, false on failure (read error or number of entries don't
+ * match)
+ */
+ bool
+ loadTrainingFeatures(const std::string& file_name,
+ const std::string& labels_file_name)
+ {
+ FeatureCloudPtr cloud(new FeatureCloud);
+ if (pcl::io::loadPCDFile(file_name, *cloud) != 0)
+ return false;
+ std::vector<std::string> labels;
+ std::ifstream f(labels_file_name.c_str());
+ std::string label;
+ while (getline(f, label))
+ if (!label.empty())
+ labels.push_back(label);
+ return addTrainingFeatures(cloud, labels);
+ }
- /** \brief Add the feature extracted from the cloud at the specified
- * location as a training example with the given labels.
- * \note this function has a cumulative effect.
- * \param file_name PCD file containing the training data
- * \param label the class label for the training example
- * \return true on success, false on failure (read error or number of entries don't match)
- */
- bool loadTrainingData (const std::string& file_name, std::string label)
- {
- pcl::PCLPointCloud2 cloud_blob;
- if (pcl::io::loadPCDFile (file_name, cloud_blob) != 0)
- return false;
- return addTrainingData (cloud_blob, label);
- }
+ /**
+ * \brief Add the feature extracted from the cloud at the specified
+ * location as a training example with the given labels.
+ * \note this function has a cumulative effect.
+ * \param file_name PCD file containing the training data
+ * \param label the class label for the training example
+ * \return true on success, false on failure (read error or number of entries don't
+ * match)
+ */
+ bool
+ loadTrainingData(const std::string& file_name, std::string label)
+ {
+ pcl::PCLPointCloud2 cloud_blob;
+ if (pcl::io::loadPCDFile(file_name, cloud_blob) != 0)
+ return false;
+ return addTrainingData(cloud_blob, label);
+ }
- /** \brief Add the feature extracted from the cloud as a training example with the given labels.
- * \note this function has a cumulative effect.
- * \param training_data point cloud for training feature extraction
- * \param label the class label for the training example
- * \return true on success, false on failure (read error or number of entries don't match)
- */
- bool addTrainingData (const pcl::PCLPointCloud2 &training_data, std::string &label)
- {
- // Create label list containing the single label
- std::vector<std::string> labels;
- labels.push_back (label);
+ /**
+ * \brief Add the feature extracted from the cloud as a training example with the
+ * given labels.
+ * \note this function has a cumulative effect.
+ * \param training_data point cloud for training feature extraction
+ * \param label the class label for the training example
+ * \return true on success, false on failure (read error or number of entries
+ * don't match)
+ */
+ bool
+ addTrainingData(const pcl::PCLPointCloud2& training_data, std::string& label)
+ {
+ // Create label list containing the single label
+ std::vector<std::string> labels;
+ labels.push_back(label);
- // Compute the feature from the cloud and add it as a training example
- FeatureCloudPtr vfhs = computeFeature (training_data);
- return addTrainingFeatures(vfhs, labels);
- }
+ // Compute the feature from the cloud and add it as a training example
+ FeatureCloudPtr vfhs = computeFeature(training_data);
+ return addTrainingFeatures(vfhs, labels);
+ }
- /** \brief Utility function for the default classification process.
- * \param testing_data the point clouds to be classified
- * \param radius the maximum search radius in feature space -- 300 by default
- * \param minimum_score the score to be given to matches at maximum distance (>0) -- 0.002 by default
- * \return pair of label and score for each relevant training class
- */
- ResultPtr classify (const pcl::PCLPointCloud2 &testing_data, double radius = 300, double min_score = 0.002)
- {
- // compute the VFH feature for this point cloud
- FeatureCloudPtr vfhs = computeFeature (testing_data);
- // compute gaussian parameter producing the desired minimum score (around 50 for the default values)
- float gaussian_param = - static_cast<float> (radius / std::log (min_score));
- // TODO accept result to be filled in by reference
- return classifier_.classify(vfhs->points.at (0), radius, gaussian_param);
- }
+ /**
+ * \brief Utility function for the default classification process.
+ * \param testing_data the point clouds to be classified
+ * \param radius the maximum search radius in feature space -- 300 by default
+ * \param minimum_score the score to be given to matches at maximum distance
+ * (>0) -- 0.002 by default
+ * \return pair of label and score for each relevant training class
+ */
+ ResultPtr
+ classify(const pcl::PCLPointCloud2& testing_data,
+ double radius = 300,
+ double min_score = 0.002)
+ {
+ // compute the VFH feature for this point cloud
+ FeatureCloudPtr vfhs = computeFeature(testing_data);
+ // compute gaussian parameter producing the desired minimum score
+ // (around 50 for the default values)
+ float gaussian_param = -static_cast<float>(radius / std::log(min_score));
+ // TODO accept result to be filled in by reference
+ return classifier_.classify(vfhs->points.at(0), radius, gaussian_param);
+ }
- /** \brief Extract the VFH feature describing the given point cloud.
- * \param points point cloud for feature extraction
- * \param radius search radius for normal estimation -- 0.03 m by default
- * \return point cloud containing the extracted feature
- */
- FeatureCloudPtr computeFeature (const pcl::PCLPointCloud2 &points, double radius = 0.03)
- {
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
- pcl::fromPCLPointCloud2 (points, *cloud);
- return pcl::computeVFH<pcl::PointXYZ> (cloud, radius);
- }
- };
-}
+ /**
+ * \brief Extract the VFH feature describing the given point cloud.
+ * \param points point cloud for feature extraction
+ * \param radius search radius for normal estimation -- 0.03 m by default
+ * \return point cloud containing the extracted feature
+ */
+ FeatureCloudPtr
+ computeFeature(const pcl::PCLPointCloud2& points, double radius = 0.03)
+ {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
+ pcl::fromPCLPointCloud2(points, *cloud);
+ return pcl::computeVFH<pcl::PointXYZ>(cloud, radius);
+ }
+};
+
+} // namespace pcl
class QMenu;
class QPoint;
-namespace Ui
-{
- class MainWindow;
+
+namespace Ui {
+class MainWindow;
}
-namespace pcl
-{
- namespace modeler
- {
- class ParameterDialog;
+namespace pcl {
+namespace modeler {
- class AbstractItem
- {
- public:
- AbstractItem();
- ~AbstractItem();
+class ParameterDialog;
- void
- showContextMenu(const QPoint* position);
+class AbstractItem {
+public:
+ AbstractItem();
+ ~AbstractItem();
- virtual std::string
- getItemName() const = 0;
+ void
+ showContextMenu(const QPoint* position);
- void
- showPropertyEditor();
+ virtual std::string
+ getItemName() const = 0;
- protected:
- Ui::MainWindow* ui() const;
+ void
+ showPropertyEditor();
- virtual void
- prepareContextMenu(QMenu* menu) const = 0;
+protected:
+ Ui::MainWindow*
+ ui() const;
- virtual void
- prepareProperties(ParameterDialog* parameter_dialog) = 0;
+ virtual void
+ prepareContextMenu(QMenu* menu) const = 0;
- virtual void
- setProperties() = 0;
- };
+ virtual void
+ prepareProperties(ParameterDialog* parameter_dialog) = 0;
- }
-}
+ virtual void
+ setProperties() = 0;
+};
+
+} // namespace modeler
+} // namespace pcl
#include <QObject>
-namespace pcl
-{
- namespace modeler
- {
- class CloudMeshItem;
- class ParameterDialog;
+namespace pcl {
+namespace modeler {
- class AbstractWorker : public QObject
- {
- Q_OBJECT
+class CloudMeshItem;
+class ParameterDialog;
- public:
- AbstractWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent=nullptr);
- ~AbstractWorker();
+class AbstractWorker : public QObject {
+ Q_OBJECT
- int
- exec();
+public:
+ AbstractWorker(const QList<CloudMeshItem*>& cloud_mesh_items,
+ QWidget* parent = nullptr);
+ ~AbstractWorker();
- public Q_SLOTS:
- void
- process();
+ int
+ exec();
- Q_SIGNALS:
- void
- dataUpdated(CloudMeshItem* cloud_mesh_item);
+public Q_SLOTS:
+ void
+ process();
- void
- finished();
+Q_SIGNALS:
+ void
+ dataUpdated(CloudMeshItem* cloud_mesh_item);
- protected:
- void
- emitDataUpdated(CloudMeshItem* cloud_mesh_item);
+ void
+ finished();
- virtual std::string
- getName () const {return ("");}
+protected:
+ void
+ emitDataUpdated(CloudMeshItem* cloud_mesh_item);
- virtual void
- initParameters(CloudMeshItem* cloud_mesh_item) = 0;
+ virtual std::string
+ getName() const
+ {
+ return "";
+ }
- virtual void
- setupParameters() = 0;
+ virtual void
+ initParameters(CloudMeshItem* cloud_mesh_item) = 0;
- virtual void
- processImpl(CloudMeshItem* cloud_mesh_item) = 0;
+ virtual void
+ setupParameters() = 0;
- protected:
- QList<CloudMeshItem*> cloud_mesh_items_;
- ParameterDialog* parameter_dialog_;
- };
+ virtual void
+ processImpl(CloudMeshItem* cloud_mesh_item) = 0;
- }
-}
+protected:
+ QList<CloudMeshItem*> cloud_mesh_items_;
+ ParameterDialog* parameter_dialog_;
+};
+
+} // namespace modeler
+} // namespace pcl
#pragma once
+#include <pcl/apps/modeler/abstract_item.h>
+#include <pcl/apps/modeler/cloud_mesh.h>
+#include <pcl/common/eigen.h>
+
#include <QTreeWidgetItem>
#include <vtkSmartPointer.h>
-#include <pcl/common/eigen.h>
-#include <pcl/apps/modeler/abstract_item.h>
-#include <pcl/apps/modeler/cloud_mesh.h>
-
class vtkActor;
class vtkPolyData;
class vtkMatrix4x4;
class vtkRenderWindow;
+namespace pcl {
+namespace modeler {
+
+class ChannelActorItem : public QTreeWidgetItem, public AbstractItem {
+public:
+ ChannelActorItem(QTreeWidgetItem* parent,
+ const CloudMesh::Ptr& cloud_mesh,
+ const vtkSmartPointer<vtkRenderWindow>& render_window,
+ const vtkSmartPointer<vtkActor>& actor,
+ const std::string& channel_name);
+ ~ChannelActorItem();
+
+ void
+ init();
+
+ void
+ update();
+
+ void
+ switchRenderWindow(vtkRenderWindow* render_window);
+
+protected:
+ void
+ attachActor();
+
+ void
+ detachActor();
+
+ virtual void
+ initImpl() = 0;
+
+ virtual void
+ updateImpl() = 0;
+
+ void
+ prepareContextMenu(QMenu* menu) const override;
+
+ CloudMesh::Ptr cloud_mesh_;
+ vtkSmartPointer<vtkPolyData> poly_data_;
+ vtkSmartPointer<vtkRenderWindow> render_window_;
+ std::string color_scheme_;
+ vtkSmartPointer<vtkActor> actor_;
+ double r_, g_, b_;
+
+private:
+};
-namespace pcl
-{
- namespace modeler
- {
- class ChannelActorItem : public QTreeWidgetItem, public AbstractItem
- {
- public:
- ChannelActorItem(QTreeWidgetItem* parent,
- const CloudMesh::Ptr& cloud_mesh,
- const vtkSmartPointer<vtkRenderWindow>& render_window,
- const vtkSmartPointer<vtkActor>& actor,
- const std::string& channel_name);
- ~ChannelActorItem();
-
- void
- init();
-
- void
- update();
-
- void
- switchRenderWindow(vtkRenderWindow* render_window);
-
- protected:
- void
- attachActor();
-
- void
- detachActor();
-
- virtual void
- initImpl() = 0;
-
- virtual void
- updateImpl() = 0;
-
- void
- prepareContextMenu(QMenu* menu) const override;
-
- CloudMesh::Ptr cloud_mesh_;
- vtkSmartPointer<vtkPolyData> poly_data_;
- vtkSmartPointer<vtkRenderWindow> render_window_;
- std::string color_scheme_;
- vtkSmartPointer<vtkActor> actor_;
- double r_, g_, b_;
-
- private:
- };
- }
-}
+} // namespace modeler
+} // namespace pcl
#pragma once
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
#include <pcl/Vertices.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
#include <vtkSmartPointer.h>
class vtkPoints;
class vtkCellArray;
class vtkDataArray;
-namespace pcl
-{
- namespace modeler
- {
- class CloudMesh
- {
- public:
- using PointT = pcl::PointSurfel;
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudPtr = PointCloud::Ptr;
- using PointCloudConstPtr = PointCloud::ConstPtr;
- using Ptr = std::shared_ptr<CloudMesh>;
+namespace pcl {
+namespace modeler {
- CloudMesh ();
- CloudMesh (PointCloudPtr cloud);
- ~CloudMesh ();
+class CloudMesh {
+public:
+ using PointT = pcl::PointSurfel;
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudPtr = PointCloud::Ptr;
+ using PointCloudConstPtr = PointCloud::ConstPtr;
+ using Ptr = std::shared_ptr<CloudMesh>;
- PointCloudPtr&
- getCloud() {return cloud_;}
- PointCloudConstPtr
- getCloud() const {return cloud_;}
+ CloudMesh();
+ CloudMesh(PointCloudPtr cloud);
+ ~CloudMesh();
- std::vector<pcl::Vertices>&
- getPolygons() {return polygons_;}
- const std::vector<pcl::Vertices>&
- getPolygons() const {return polygons_;}
+ PointCloudPtr&
+ getCloud()
+ {
+ return cloud_;
+ }
+ PointCloudConstPtr
+ getCloud() const
+ {
+ return cloud_;
+ }
- vtkSmartPointer<vtkPoints>&
- getVtkPoints() {return vtk_points_;}
- const vtkSmartPointer<vtkPoints>&
- getVtkPoints() const {return vtk_points_;}
+ std::vector<pcl::Vertices>&
+ getPolygons()
+ {
+ return polygons_;
+ }
+ const std::vector<pcl::Vertices>&
+ getPolygons() const
+ {
+ return polygons_;
+ }
- vtkSmartPointer<vtkCellArray>&
- getVtkPolygons() {return vtk_polygons_;}
- const vtkSmartPointer<vtkCellArray>&
- getVtkPolygons() const {return vtk_polygons_;}
+ vtkSmartPointer<vtkPoints>&
+ getVtkPoints()
+ {
+ return vtk_points_;
+ }
+ const vtkSmartPointer<vtkPoints>&
+ getVtkPoints() const
+ {
+ return vtk_points_;
+ }
- std::vector<std::string>
- getAvaiableFieldNames() const;
+ vtkSmartPointer<vtkCellArray>&
+ getVtkPolygons()
+ {
+ return vtk_polygons_;
+ }
+ const vtkSmartPointer<vtkCellArray>&
+ getVtkPolygons() const
+ {
+ return vtk_polygons_;
+ }
- bool
- open(const std::string& filename);
+ std::vector<std::string>
+ getAvaiableFieldNames() const;
- bool
- save(const std::string& filename) const;
+ bool
+ open(const std::string& filename);
- static bool
- save(const std::vector<const CloudMesh*>& cloud_meshes, const std::string& filename);
+ bool
+ save(const std::string& filename) const;
- void
- getColorScalarsFromField(vtkSmartPointer<vtkDataArray> &scalars, const std::string& field) const;
+ static bool
+ save(const std::vector<const CloudMesh*>& cloud_meshes, const std::string& filename);
- void
- updateVtkPoints();
+ void
+ getColorScalarsFromField(vtkSmartPointer<vtkDataArray>& scalars,
+ const std::string& field) const;
- void
- updateVtkPolygons();
+ void
+ updateVtkPoints();
- void
- transform(double tx, double ty, double tz, double rx, double ry, double rz);
+ void
+ updateVtkPolygons();
- protected:
+ void
+ transform(double tx, double ty, double tz, double rx, double ry, double rz);
+protected:
+private:
+ PointCloudPtr cloud_;
+ std::vector<pcl::Vertices> polygons_;
- private:
- PointCloudPtr cloud_;
- std::vector<pcl::Vertices> polygons_;
+ vtkSmartPointer<vtkPoints> vtk_points_;
+ vtkSmartPointer<vtkCellArray> vtk_polygons_;
+};
- vtkSmartPointer<vtkPoints> vtk_points_;
- vtkSmartPointer<vtkCellArray> vtk_polygons_;
- };
- }
-}
+} // namespace modeler
+} // namespace pcl
#pragma once
-#include <QTreeWidgetItem>
-
#include <pcl/apps/modeler/abstract_item.h>
#include <pcl/apps/modeler/cloud_mesh.h>
-namespace pcl
-{
- namespace modeler
+#include <QTreeWidgetItem>
+
+namespace pcl {
+namespace modeler {
+
+class CloudMesh;
+class DoubleParameter;
+
+class CloudMeshItem : public QTreeWidgetItem, public AbstractItem {
+public:
+ CloudMeshItem(QTreeWidgetItem* parent, const std::string& filename);
+ CloudMeshItem(QTreeWidgetItem* parent, CloudMesh::PointCloudPtr cloud);
+ CloudMeshItem(QTreeWidgetItem* parent, const CloudMeshItem& cloud_mesh_item);
+ ~CloudMeshItem();
+
+ inline CloudMesh::Ptr&
+ getCloudMesh()
+ {
+ return cloud_mesh_;
+ }
+
+ inline const CloudMesh::Ptr&
+ getCloudMesh() const
+ {
+ return cloud_mesh_;
+ }
+
+ static bool
+ savePointCloud(const QList<CloudMeshItem*>& items, const QString& filename);
+
+ bool
+ open();
+
+ void
+ createChannels();
+
+ void
+ updateChannels();
+
+ std::string
+ getItemName() const override
{
- class CloudMesh;
- class DoubleParameter;
-
- class CloudMeshItem : public QTreeWidgetItem, public AbstractItem
- {
- public:
- CloudMeshItem(QTreeWidgetItem* parent, const std::string& filename);
- CloudMeshItem(QTreeWidgetItem* parent, CloudMesh::PointCloudPtr cloud);
- CloudMeshItem(QTreeWidgetItem* parent, const CloudMeshItem& cloud_mesh_item);
- ~CloudMeshItem();
-
- inline CloudMesh::Ptr&
- getCloudMesh()
- {
- return cloud_mesh_;
- }
-
- inline const CloudMesh::Ptr&
- getCloudMesh() const
- {
- return cloud_mesh_;
- }
-
- static bool
- savePointCloud(const QList<CloudMeshItem*>& items, const QString& filename);
-
- bool
- open();
-
- void
- createChannels();
-
- void
- updateChannels();
-
- std::string
- getItemName() const override {return "Cloud Mesh Item";}
-
- void
- updateRenderWindow();
-
- protected:
- void
- prepareContextMenu(QMenu* menu) const override;
-
- void
- prepareProperties(ParameterDialog* parameter_dialog) override;
-
- void
- setProperties() override;
-
- private:
- std::string filename_;
- CloudMesh::Ptr cloud_mesh_;
-
- DoubleParameter* translation_x_;
- DoubleParameter* translation_y_;
- DoubleParameter* translation_z_;
- DoubleParameter* rotation_x_;
- DoubleParameter* rotation_y_;
- DoubleParameter* rotation_z_;
- };
+ return "Cloud Mesh Item";
}
-}
+
+ void
+ updateRenderWindow();
+
+protected:
+ void
+ prepareContextMenu(QMenu* menu) const override;
+
+ void
+ prepareProperties(ParameterDialog* parameter_dialog) override;
+
+ void
+ setProperties() override;
+
+private:
+ std::string filename_;
+ CloudMesh::Ptr cloud_mesh_;
+
+ DoubleParameter* translation_x_;
+ DoubleParameter* translation_y_;
+ DoubleParameter* translation_z_;
+ DoubleParameter* rotation_x_;
+ DoubleParameter* rotation_y_;
+ DoubleParameter* rotation_z_;
+};
+
+} // namespace modeler
+} // namespace pcl
#include <QObject>
-namespace pcl
-{
- namespace modeler
- {
- class CloudMeshItem;
-
- class CloudMeshItemUpdater : public QObject
- {
- Q_OBJECT
-
- public:
- CloudMeshItemUpdater (CloudMeshItem* cloud_mesh_item);
- ~CloudMeshItemUpdater ();
-
- public Q_SLOTS:
- void
- updateCloudMeshItem();
-
- private:
- CloudMeshItem* cloud_mesh_item_;
- };
- }
-}
+namespace pcl {
+namespace modeler {
+
+class CloudMeshItem;
+
+class CloudMeshItemUpdater : public QObject {
+ Q_OBJECT
+
+public:
+ CloudMeshItemUpdater(CloudMeshItem* cloud_mesh_item);
+ ~CloudMeshItemUpdater();
+
+public Q_SLOTS:
+ void
+ updateCloudMeshItem();
+
+private:
+ CloudMeshItem* cloud_mesh_item_;
+};
+
+} // namespace modeler
+} // namespace pcl
#include <QDockWidget>
-namespace pcl
-{
- namespace modeler
- {
- class DockWidget : public QDockWidget
- {
- public:
- explicit DockWidget(const QString &title, QWidget *parent = nullptr, Qt::WindowFlags flags = Qt::WindowFlags());
- explicit DockWidget(QWidget *parent = nullptr, Qt::WindowFlags flags = Qt::WindowFlags());
- ~DockWidget();
-
- void
- setFocusBasedStyle(bool focused);
- protected:
- void
- focusInEvent ( QFocusEvent * event ) override;
-
- private:
- };
- }
-}
+namespace pcl {
+namespace modeler {
+
+class DockWidget : public QDockWidget {
+public:
+ explicit DockWidget(const QString& title,
+ QWidget* parent = nullptr,
+ Qt::WindowFlags flags = Qt::WindowFlags());
+ explicit DockWidget(QWidget* parent = nullptr,
+ Qt::WindowFlags flags = Qt::WindowFlags());
+ ~DockWidget();
+
+ void
+ setFocusBasedStyle(bool focused);
+
+protected:
+ void
+ focusInEvent(QFocusEvent* event) override;
+
+private:
+};
+
+} // namespace modeler
+} // namespace pcl
#include <pcl/apps/modeler/abstract_worker.h>
#include <pcl/apps/modeler/cloud_mesh.h>
-namespace pcl
-{
- namespace modeler
- {
- class IntParameter;
- class DoubleParameter;
+namespace pcl {
+namespace modeler {
- class ICPRegistrationWorker : public AbstractWorker
- {
- public:
- ICPRegistrationWorker(CloudMesh::PointCloudPtr cloud, const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent=nullptr);
- ~ICPRegistrationWorker();
+class IntParameter;
+class DoubleParameter;
- protected:
- std::string
- getName () const override {return ("Normal Estimation");}
+class ICPRegistrationWorker : public AbstractWorker {
+public:
+ ICPRegistrationWorker(CloudMesh::PointCloudPtr cloud,
+ const QList<CloudMeshItem*>& cloud_mesh_items,
+ QWidget* parent = nullptr);
+ ~ICPRegistrationWorker();
- void
- initParameters(CloudMeshItem* cloud_mesh_item) override;
+protected:
+ std::string
+ getName() const override
+ {
+ return "Normal Estimation";
+ }
- void
- setupParameters() override;
+ void
+ initParameters(CloudMeshItem* cloud_mesh_item) override;
- void
- processImpl(CloudMeshItem* cloud_mesh_item) override;
+ void
+ setupParameters() override;
- private:
- CloudMesh::PointCloudPtr cloud_;
+ void
+ processImpl(CloudMeshItem* cloud_mesh_item) override;
- double x_min_, x_max_;
- double y_min_, y_max_;
- double z_min_, z_max_;
+private:
+ CloudMesh::PointCloudPtr cloud_;
- DoubleParameter* max_correspondence_distance_;
- IntParameter* max_iterations_;
- DoubleParameter* transformation_epsilon_;
- DoubleParameter* euclidean_fitness_epsilon_;
- };
+ double x_min_, x_max_;
+ double y_min_, y_max_;
+ double z_min_, z_max_;
- }
-}
+ DoubleParameter* max_correspondence_distance_;
+ IntParameter* max_iterations_;
+ DoubleParameter* transformation_epsilon_;
+ DoubleParameter* euclidean_fitness_epsilon_;
+};
+
+} // namespace modeler
+} // namespace pcl
/*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2010, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-*
-*/
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
#ifndef PCL_MODELER_PARAMETER_IMPL_H_
#define PCL_MODELER_PARAMETER_IMPL_H_
#include <pcl/apps/modeler/parameter.h>
+namespace pcl {
+namespace modeler {
-namespace pcl
-{
- namespace modeler
- {
//////////////////////////////////////////////////////////////////////////////////////////////
- template <class T> std::string
- EnumParameter<T>::valueTip()
- {
- std::string tip("possible values: {");
- typename std::map<T, std::string>::const_iterator it = candidates_.begin();
- do
- {
- tip += it->second;
- ++ it;
- if (it != candidates_.end())
- tip += ", ";
- } while(it != candidates_.end());
- tip += "}";
+template <class T>
+std::string
+EnumParameter<T>::valueTip()
+{
+ std::string tip("possible values: {");
+ typename std::map<T, std::string>::const_iterator it = candidates_.begin();
+ do {
+ tip += it->second;
+ ++it;
+ if (it != candidates_.end())
+ tip += ", ";
+ } while (it != candidates_.end());
+ tip += "}";
- return (tip);
- }
+ return tip;
+}
//////////////////////////////////////////////////////////////////////////////////////////////
- template <class T> QWidget *
- EnumParameter<T>::createEditor(QWidget *parent)
- {
- QComboBox* editor = new QComboBox(parent);
- for (typename std::map<T, std::string>::const_iterator it = candidates_.begin();
- it != candidates_.end();
- ++ it)
- {
- editor->addItem(it->second.c_str());
- }
+template <class T>
+QWidget*
+EnumParameter<T>::createEditor(QWidget* parent)
+{
+ QComboBox* editor = new QComboBox(parent);
+ for (typename std::map<T, std::string>::const_iterator it = candidates_.begin();
+ it != candidates_.end();
+ ++it) {
+ editor->addItem(it->second.c_str());
+ }
- return (editor);
- }
+ return editor;
+}
//////////////////////////////////////////////////////////////////////////////////////////////
- template <class T> void
- EnumParameter<T>::setEditorData(QWidget *editor)
- {
- QComboBox *comboBox = static_cast<QComboBox*>(editor);
+template <class T>
+void
+EnumParameter<T>::setEditorData(QWidget* editor)
+{
+ QComboBox* comboBox = static_cast<QComboBox*>(editor);
- T value = T (*this);
- comboBox->setCurrentIndex(value);
- }
+ T value = T(*this);
+ comboBox->setCurrentIndex(value);
+}
//////////////////////////////////////////////////////////////////////////////////////////////
- template <class T> void
- EnumParameter<T>::getEditorData(QWidget *editor)
- {
- QComboBox *comboBox = static_cast<QComboBox*>(editor);
- T value = T (comboBox->currentIndex());
- current_value_ = value;
- }
+template <class T>
+void
+EnumParameter<T>::getEditorData(QWidget* editor)
+{
+ QComboBox* comboBox = static_cast<QComboBox*>(editor);
+ T value = T(comboBox->currentIndex());
+ current_value_ = value;
+}
//////////////////////////////////////////////////////////////////////////////////////////////
- template <class T> std::pair<QVariant, int>
- EnumParameter<T>::toModelData()
- {
- std::pair<QVariant, int> model_data;
- for (typename std::map<T, std::string>::const_iterator it = candidates_.begin();
- it != candidates_.end();
- ++ it)
- {
- if (it->first == value)
- {
- model_data.first = it->second;
- break;
- }
- }
- model_data.second = Qt::EditRole;
-
- return (model_data);
+template <class T>
+std::pair<QVariant, int>
+EnumParameter<T>::toModelData()
+{
+ std::pair<QVariant, int> model_data;
+ for (typename std::map<T, std::string>::const_iterator it = candidates_.begin();
+ it != candidates_.end();
+ ++it) {
+ if (it->first == value) {
+ model_data.first = it->second;
+ break;
}
}
+ model_data.second = Qt::EditRole;
+
+ return model_data;
}
+} // namespace modeler
+} // namespace pcl
+
#endif // PCL_MODELER_PARAMETER_IMPL_H_
#include <pcl/apps/modeler/scene_tree.h>
-namespace pcl
-{
- namespace modeler
- {
- /////////////////////////////////////////////////////////////////////////////////////////////
- template <class T> QList<T*>
- pcl::modeler::SceneTree::selectedTypeItems() const
- {
- QList<QTreeWidgetItem*> selected_items = selectedItems();
- QList<T*> selected_t_items;
- for (auto &selected_item : selected_items)
- {
- T* t_item = dynamic_cast<T*>(selected_item);
- if(t_item != nullptr)
- selected_t_items.push_back(t_item);
- }
+namespace pcl {
+namespace modeler {
- return (selected_t_items);
- }
+/////////////////////////////////////////////////////////////////////////////////////////////
+template <class T>
+QList<T*>
+pcl::modeler::SceneTree::selectedTypeItems() const
+{
+ QList<QTreeWidgetItem*> selected_items = selectedItems();
+ QList<T*> selected_t_items;
+ for (auto& selected_item : selected_items) {
+ T* t_item = dynamic_cast<T*>(selected_item);
+ if (t_item != nullptr)
+ selected_t_items.push_back(t_item);
}
+
+ return selected_t_items;
}
+} // namespace modeler
+} // namespace pcl
#endif // PCL_MODELER_SCENE_TREE_IMPL_H_
#include <memory>
-namespace pcl
-{
- namespace modeler
+namespace pcl {
+namespace modeler {
+
+class SceneTree;
+class AbstractItem;
+
+class MainWindow : public QMainWindow {
+ Q_OBJECT
+
+public:
+ static MainWindow&
+ getInstance()
{
- class SceneTree;
- class AbstractItem;
-
- class MainWindow : public QMainWindow
- {
- Q_OBJECT
-
- public:
- static MainWindow& getInstance() {
- static MainWindow theSingleton;
- return theSingleton;
- }
-
- QString
- getRecentFolder();
-
- RenderWindowItem*
- createRenderWindow();
-
- public Q_SLOTS:
- // slots for file menu
- void
- slotOpenProject();
- void
- slotSaveProject();
- void
- slotCloseProject();
- void
- slotExit();
- void
- slotUpdateRecentFile(const QString& filename);
-
- // slots for view menu
- void
- slotCreateRenderWindow();
-
- void
- slotOnWorkerStarted();
-
- void
- slotOnWorkerFinished();
-
- private:
- // methods for file Menu
- void
- connectFileMenuActions();
- void
- createRecentPointCloudActions();
- void
- updateRecentPointCloudActions();
- void
- createRecentProjectActions();
- void
- updateRecentProjectActions();
- bool
- openProjectImpl(const QString& filename);
- static void
- updateRecentActions(std::vector<std::shared_ptr<QAction>>& recent_actions, QStringList& recent_items);
-
- // methods for view menu
- void
- connectViewMenuActions();
-
- // methods for edit menu
- void
- connectEditMenuActions();
-
- // methods for global settings
- void
- loadGlobalSettings();
- void
- saveGlobalSettings();
-
- private Q_SLOTS:
- void
- slotOpenRecentPointCloud();
- void
- slotOpenRecentProject();
-
- private:
- friend class AbstractItem;
-
- MainWindow();
- MainWindow(const MainWindow &) = delete;
- MainWindow& operator=(const MainWindow &) = delete;
- ~MainWindow();
-
- Ui::MainWindow *ui_; // Designer form
-
- // shortcuts for recent point clouds/projects
- QStringList recent_files_;
- QStringList recent_projects_;
- static const std::size_t MAX_RECENT_NUMBER = 8;
- std::vector<std::shared_ptr<QAction>> recent_pointcloud_actions_;
- std::vector<std::shared_ptr<QAction>> recent_project_actions_;
- };
+ static MainWindow theSingleton;
+ return theSingleton;
}
-}
+
+ QString
+ getRecentFolder();
+
+ RenderWindowItem*
+ createRenderWindow();
+
+public Q_SLOTS:
+ // slots for file menu
+ void
+ slotOpenProject();
+ void
+ slotSaveProject();
+ void
+ slotCloseProject();
+ void
+ slotExit();
+ void
+ slotUpdateRecentFile(const QString& filename);
+
+ // slots for view menu
+ void
+ slotCreateRenderWindow();
+
+ void
+ slotOnWorkerStarted();
+
+ void
+ slotOnWorkerFinished();
+
+private:
+ // methods for file Menu
+ void
+ connectFileMenuActions();
+ void
+ createRecentPointCloudActions();
+ void
+ updateRecentPointCloudActions();
+ void
+ createRecentProjectActions();
+ void
+ updateRecentProjectActions();
+ bool
+ openProjectImpl(const QString& filename);
+ static void
+ updateRecentActions(std::vector<std::shared_ptr<QAction>>& recent_actions,
+ QStringList& recent_items);
+
+ // methods for view menu
+ void
+ connectViewMenuActions();
+
+ // methods for edit menu
+ void
+ connectEditMenuActions();
+
+ // methods for global settings
+ void
+ loadGlobalSettings();
+ void
+ saveGlobalSettings();
+
+private Q_SLOTS:
+ void
+ slotOpenRecentPointCloud();
+ void
+ slotOpenRecentProject();
+
+private:
+ friend class AbstractItem;
+
+ MainWindow();
+ MainWindow(const MainWindow&) = delete;
+ MainWindow&
+ operator=(const MainWindow&) = delete;
+ ~MainWindow();
+
+ Ui::MainWindow* ui_; // Designer form
+
+ // shortcuts for recent point clouds/projects
+ QStringList recent_files_;
+ QStringList recent_projects_;
+ static const std::size_t MAX_RECENT_NUMBER = 8;
+ std::vector<std::shared_ptr<QAction>> recent_pointcloud_actions_;
+ std::vector<std::shared_ptr<QAction>> recent_project_actions_;
+};
+
+} // namespace modeler
+} // namespace pcl
#include <pcl/apps/modeler/abstract_worker.h>
-namespace pcl
-{
- namespace modeler
- {
- class DoubleParameter;
+namespace pcl {
+namespace modeler {
- class NormalEstimationWorker : public AbstractWorker
- {
- public:
- NormalEstimationWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent = nullptr);
- ~NormalEstimationWorker();
+class DoubleParameter;
- protected:
- std::string
- getName () const override { return ("Normal Estimation"); }
+class NormalEstimationWorker : public AbstractWorker {
+public:
+ NormalEstimationWorker(const QList<CloudMeshItem*>& cloud_mesh_items,
+ QWidget* parent = nullptr);
+ ~NormalEstimationWorker();
- void
- initParameters(CloudMeshItem* cloud_mesh_item) override;
+protected:
+ std::string
+ getName() const override
+ {
+ return "Normal Estimation";
+ }
- void
- setupParameters() override;
+ void
+ initParameters(CloudMeshItem* cloud_mesh_item) override;
- void
- processImpl(CloudMeshItem* cloud_mesh_item) override;
+ void
+ setupParameters() override;
- private:
- double x_min_, x_max_;
- double y_min_, y_max_;
- double z_min_, z_max_;
+ void
+ processImpl(CloudMeshItem* cloud_mesh_item) override;
- DoubleParameter* search_radius_;
- };
+private:
+ double x_min_, x_max_;
+ double y_min_, y_max_;
+ double z_min_, z_max_;
- }
-}
+ DoubleParameter* search_radius_;
+};
+} // namespace modeler
+} // namespace pcl
class vtkIdTypeArray;
-namespace pcl
-{
- namespace modeler
+namespace pcl {
+namespace modeler {
+
+class NormalsActorItem : public ChannelActorItem {
+public:
+ NormalsActorItem(QTreeWidgetItem* parent,
+ const CloudMesh::Ptr& cloud_mesh,
+ const vtkSmartPointer<vtkRenderWindow>& render_window);
+ ~NormalsActorItem();
+
+ std::string
+ getItemName() const override
{
- class NormalsActorItem : public ChannelActorItem
- {
- public:
- NormalsActorItem(QTreeWidgetItem* parent,
- const CloudMesh::Ptr& cloud_mesh,
- const vtkSmartPointer<vtkRenderWindow>& render_window);
- ~NormalsActorItem ();
+ return "Points Actor Item";
+ }
- std::string
- getItemName() const override {return "Points Actor Item";}
+protected:
+ void
+ createNormalLines();
- protected:
- void
- createNormalLines();
+ void
+ initImpl() override;
- void
- initImpl() override;
+ void
+ updateImpl() override;
- void
- updateImpl() override;
+ void
+ prepareContextMenu(QMenu* menu) const override;
- void
- prepareContextMenu(QMenu* menu) const override;
+ void
+ prepareProperties(ParameterDialog* parameter_dialog) override;
- void
- prepareProperties(ParameterDialog* parameter_dialog) override;
+ void
+ setProperties() override;
- void
- setProperties() override;
+private:
+ double level_;
+ double scale_;
+};
- private:
- double level_;
- double scale_;
- };
- }
-}
+} // namespace modeler
+} // namespace pcl
/*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2010, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-*
-*/
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
#pragma once
-#include <map>
-#include <string>
-
#include <boost/any.hpp>
#include <QColor>
#include <QVariant>
+#include <map>
+#include <string>
+
class QAbstractItemModel;
class QWidget;
-namespace pcl
-{
- namespace modeler
+namespace pcl {
+namespace modeler {
+
+class Parameter {
+public:
+ Parameter(const std::string& name,
+ const std::string& description,
+ const boost::any& value)
+ : name_(name), description_(description), default_value_(value), current_value_(value)
+ {}
+ ~Parameter() {}
+
+ const std::string&
+ getName() const
+ {
+ return name_;
+ }
+
+ const std::string&
+ getDescription() const
+ {
+ return description_;
+ }
+
+ void
+ setDefaultValue(const boost::any& value)
+ {
+ default_value_ = value;
+ }
+
+ void
+ reset()
{
- class Parameter
- {
- public:
- Parameter(const std::string& name, const std::string& description, const boost::any& value):
- name_(name), description_(description), default_value_(value), current_value_(value){}
- ~Parameter() {}
+ current_value_ = default_value_;
+ }
- const std::string&
- getName() const {return name_;}
+ virtual std::string
+ valueTip() = 0;
- const std::string&
- getDescription()const {return description_;}
+ virtual QWidget*
+ createEditor(QWidget* parent) = 0;
- void
- setDefaultValue(const boost::any& value)
- {
- default_value_ = value;
- }
+ virtual void
+ setEditorData(QWidget* editor) = 0;
- void
- reset() {current_value_ = default_value_;}
+ virtual void
+ setModelData(QWidget* editor, QAbstractItemModel* model, const QModelIndex& index);
- virtual std::string
- valueTip() = 0;
-
- virtual QWidget*
- createEditor(QWidget *parent) = 0;
-
- virtual void
- setEditorData(QWidget *editor) = 0;
+ virtual std::pair<QVariant, int>
+ toModelData() = 0;
- virtual void
- setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index);
+protected:
+ virtual void
+ getEditorData(QWidget* editor) = 0;
- virtual std::pair<QVariant, int>
- toModelData() = 0;
+ std::string name_;
+ std::string description_;
+ boost::any default_value_;
+ boost::any current_value_;
+};
- protected:
- virtual void
- getEditorData(QWidget *editor) = 0;
+class BoolParameter : public Parameter {
+public:
+ BoolParameter(const std::string& name, const std::string& description, bool value)
+ : Parameter(name, description, value)
+ {}
+ ~BoolParameter() {}
- std::string name_;
- std::string description_;
- boost::any default_value_;
- boost::any current_value_;
- };
+ operator bool() const { return boost::any_cast<bool>(current_value_); }
- class BoolParameter : public Parameter
- {
- public:
- BoolParameter(const std::string& name, const std::string& description, bool value):
- Parameter(name, description, value){}
- ~BoolParameter(){}
+ std::string
+ valueTip() override;
- operator bool() const {return boost::any_cast<bool>(current_value_);}
-
- std::string
- valueTip() override;
+ QWidget*
+ createEditor(QWidget* parent) override;
- QWidget*
- createEditor(QWidget *parent) override;
+ void
+ setEditorData(QWidget* editor) override;
- void
- setEditorData(QWidget *editor) override;
+ std::pair<QVariant, int>
+ toModelData() override;
- std::pair<QVariant, int>
- toModelData() override;
+protected:
+ void
+ getEditorData(QWidget* editor) override;
+};
- protected:
- void
- getEditorData(QWidget *editor) override;
- };
+class IntParameter : public Parameter {
+public:
+ IntParameter(const std::string& name,
+ const std::string& description,
+ int value,
+ int low,
+ int high,
+ int step = 1)
+ : Parameter(name, description, value), low_(low), high_(high), step_(step)
+ {}
+ virtual ~IntParameter() {}
- class IntParameter : public Parameter
- {
- public:
- IntParameter(const std::string& name, const std::string& description, int value, int low, int high, int step=1):
- Parameter(name, description, value), low_(low), high_(high), step_(step){}
- virtual ~IntParameter(){}
+ operator int() const { return boost::any_cast<int>(current_value_); }
- operator int() const {return boost::any_cast<int>(current_value_);}
+ std::string
+ valueTip() override;
- std::string
- valueTip() override;
+ QWidget*
+ createEditor(QWidget* parent) override;
- QWidget*
- createEditor(QWidget *parent) override;
-
- void
- setEditorData(QWidget *editor) override;
+ void
+ setEditorData(QWidget* editor) override;
- std::pair<QVariant, int>
- toModelData() override;
+ std::pair<QVariant, int>
+ toModelData() override;
- void
- setLow(int low)
- {
- low_ = low;
- }
-
- void
- setHigh(int high)
- {
- high_ = high;
- }
+ void
+ setLow(int low)
+ {
+ low_ = low;
+ }
- void
- setStep(int step)
- {
- step_ = step;
- }
+ void
+ setHigh(int high)
+ {
+ high_ = high;
+ }
- protected:
- void
- getEditorData(QWidget *editor) override;
+ void
+ setStep(int step)
+ {
+ step_ = step;
+ }
- int low_;
- int high_;
- int step_;
- };
+protected:
+ void
+ getEditorData(QWidget* editor) override;
- template <class T>
- class EnumParameter : public Parameter
- {
- public:
- EnumParameter(const std::string& name, const std::string& description, T value, const std::map<T, std::string>& candidates):
- Parameter(name, description, value), candidates_(candidates){}
- ~EnumParameter(){}
+ int low_;
+ int high_;
+ int step_;
+};
- operator T() const {return boost::any_cast<T>(current_value_);}
+template <class T>
+class EnumParameter : public Parameter {
+public:
+ EnumParameter(const std::string& name,
+ const std::string& description,
+ T value,
+ const std::map<T, std::string>& candidates)
+ : Parameter(name, description, value), candidates_(candidates)
+ {}
+ ~EnumParameter() {}
- std::string
- valueTip() override;
+ operator T() const { return boost::any_cast<T>(current_value_); }
- QWidget*
- createEditor(QWidget *parent) override;
+ std::string
+ valueTip() override;
- void
- setEditorData(QWidget *editor) override;
+ QWidget*
+ createEditor(QWidget* parent) override;
- std::pair<QVariant, int>
- toModelData() override;
+ void
+ setEditorData(QWidget* editor) override;
- protected:
- void
- getEditorData(QWidget *editor) override;
+ std::pair<QVariant, int>
+ toModelData() override;
- const std::map<T, std::string> candidates_;
- };
+protected:
+ void
+ getEditorData(QWidget* editor) override;
- class DoubleParameter : public Parameter
- {
- public:
- DoubleParameter(const std::string& name, const std::string& description, double value, double low, double high, double step=0.01):
- Parameter(name, description, value), low_(low), high_(high), step_(step){}
- virtual ~DoubleParameter(){}
+ const std::map<T, std::string> candidates_;
+};
- operator double() const {return boost::any_cast<double>(current_value_);}
+class DoubleParameter : public Parameter {
+public:
+ DoubleParameter(const std::string& name,
+ const std::string& description,
+ double value,
+ double low,
+ double high,
+ double step = 0.01)
+ : Parameter(name, description, value), low_(low), high_(high), step_(step)
+ {}
+ virtual ~DoubleParameter() {}
- std::string
- valueTip() override;
+ operator double() const { return boost::any_cast<double>(current_value_); }
- QWidget*
- createEditor(QWidget *parent) override;
+ std::string
+ valueTip() override;
- void
- setEditorData(QWidget *editor) override;
+ QWidget*
+ createEditor(QWidget* parent) override;
- std::pair<QVariant, int>
- toModelData() override;
+ void
+ setEditorData(QWidget* editor) override;
- void
- setLow(double low)
- {
- low_ = low;
- }
+ std::pair<QVariant, int>
+ toModelData() override;
- void
- setHigh(double high)
- {
- high_ = high;
- }
+ void
+ setLow(double low)
+ {
+ low_ = low;
+ }
- void
- setStep(double step)
- {
- step_ = step;
- }
-
- protected:
- void
- getEditorData(QWidget *editor) override;
-
- double low_;
- double high_;
- double step_;
- };
+ void
+ setHigh(double high)
+ {
+ high_ = high;
+ }
+
+ void
+ setStep(double step)
+ {
+ step_ = step;
+ }
- class ColorParameter : public Parameter
- {
- public:
- ColorParameter(const std::string& name, const std::string& description, const QColor& value):
- Parameter(name, description, value){}
- ~ColorParameter(){}
+protected:
+ void
+ getEditorData(QWidget* editor) override;
- operator QColor() const {return boost::any_cast<QColor>(current_value_);}
+ double low_;
+ double high_;
+ double step_;
+};
- std::string
- valueTip() override;
+class ColorParameter : public Parameter {
+public:
+ ColorParameter(const std::string& name,
+ const std::string& description,
+ const QColor& value)
+ : Parameter(name, description, value)
+ {}
+ ~ColorParameter() {}
- QWidget*
- createEditor(QWidget *parent) override;
+ operator QColor() const { return boost::any_cast<QColor>(current_value_); }
- void
- setEditorData(QWidget *editor) override;
+ std::string
+ valueTip() override;
- std::pair<QVariant, int>
- toModelData() override;
+ QWidget*
+ createEditor(QWidget* parent) override;
- protected:
- void
- getEditorData(QWidget *editor) override;
+ void
+ setEditorData(QWidget* editor) override;
- };
- }
-}
+ std::pair<QVariant, int>
+ toModelData() override;
+
+protected:
+ void
+ getEditorData(QWidget* editor) override;
+};
+
+} // namespace modeler
+} // namespace pcl
/*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2010, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-*
-*/
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
#pragma once
#include <QStandardItemModel>
#include <QStyledItemDelegate>
-namespace pcl
-{
- namespace modeler
- {
- class Parameter;
-
- class ParameterModel: public QStandardItemModel
- {
- public:
- ParameterModel(QObject * parent = nullptr) : QStandardItemModel(parent){}
- ParameterModel(int rows, int columns, QObject * parent = nullptr) : QStandardItemModel(rows, columns, parent){}
- ~ParameterModel() {}
-
- Qt::ItemFlags
- flags ( const QModelIndex & index ) const override
- {
- return (index.column() == 0)?(Qt::ItemIsEnabled | Qt::ItemIsSelectable):QStandardItemModel::flags(index);
- }
- };
-
- class ParameterDialog : public QDialog
- {
- Q_OBJECT
- public:
- ParameterDialog(const std::string& title, QWidget* parent=nullptr);
- ~ParameterDialog(){}
-
- void
- addParameter(Parameter* parameter);
-
- int
- exec() override;
-
- protected:
- std::map<std::string, Parameter*> name_parameter_map_;
- ParameterModel* parameter_model_;
-
- protected Q_SLOTS:
- void
- reset();
- };
+namespace pcl {
+namespace modeler {
- class ParameterDelegate : public QStyledItemDelegate
- {
- Q_OBJECT
- public:
- ParameterDelegate(std::map<std::string, Parameter*>& parameterMap, QObject *parent = nullptr);
+class Parameter;
- QWidget *
- createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const override;
-
- void
- setEditorData(QWidget *editor, const QModelIndex &index) const override;
-
- void
- setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const override;
-
- void
- updateEditorGeometry(QWidget *editor, const QStyleOptionViewItem &option, const QModelIndex &index) const override;
-
- protected:
- void
- initStyleOption(QStyleOptionViewItem *option, const QModelIndex &index) const override;
-
- private:
- Parameter*
- getCurrentParameter(const QModelIndex &index) const;
-
- std::map<std::string, Parameter*>& parameter_map_;
- };
+class ParameterModel : public QStandardItemModel {
+public:
+ ParameterModel(QObject* parent = nullptr) : QStandardItemModel(parent) {}
+ ParameterModel(int rows, int columns, QObject* parent = nullptr)
+ : QStandardItemModel(rows, columns, parent)
+ {}
+ ~ParameterModel() {}
+ Qt::ItemFlags
+ flags(const QModelIndex& index) const override
+ {
+ return (index.column() == 0) ? (Qt::ItemIsEnabled | Qt::ItemIsSelectable)
+ : QStandardItemModel::flags(index);
}
-}
+};
+
+class ParameterDialog : public QDialog {
+ Q_OBJECT
+public:
+ ParameterDialog(const std::string& title, QWidget* parent = nullptr);
+ ~ParameterDialog() {}
+
+ void
+ addParameter(Parameter* parameter);
+
+ int
+ exec() override;
+
+protected:
+ std::map<std::string, Parameter*> name_parameter_map_;
+ ParameterModel* parameter_model_;
+
+protected Q_SLOTS:
+ void
+ reset();
+};
+
+class ParameterDelegate : public QStyledItemDelegate {
+ Q_OBJECT
+public:
+ ParameterDelegate(std::map<std::string, Parameter*>& parameterMap,
+ QObject* parent = nullptr);
+
+ QWidget*
+ createEditor(QWidget* parent,
+ const QStyleOptionViewItem& option,
+ const QModelIndex& index) const override;
+
+ void
+ setEditorData(QWidget* editor, const QModelIndex& index) const override;
+
+ void
+ setModelData(QWidget* editor,
+ QAbstractItemModel* model,
+ const QModelIndex& index) const override;
+
+ void
+ updateEditorGeometry(QWidget* editor,
+ const QStyleOptionViewItem& option,
+ const QModelIndex& index) const override;
+
+protected:
+ void
+ initStyleOption(QStyleOptionViewItem* option,
+ const QModelIndex& index) const override;
+
+private:
+ Parameter*
+ getCurrentParameter(const QModelIndex& index) const;
+
+ std::map<std::string, Parameter*>& parameter_map_;
+};
+
+} // namespace modeler
+} // namespace pcl
class vtkIdTypeArray;
-namespace pcl
-{
- namespace modeler
- {
- class PointsActorItem : public ChannelActorItem
- {
- public:
- PointsActorItem(QTreeWidgetItem* parent,
- const CloudMesh::Ptr& cloud_mesh,
- const vtkSmartPointer<vtkRenderWindow>& render_window);
- ~PointsActorItem ();
+namespace pcl {
+namespace modeler {
- std::string
- getItemName() const override {return "Points Actor Item";}
+class PointsActorItem : public ChannelActorItem {
+public:
+ PointsActorItem(QTreeWidgetItem* parent,
+ const CloudMesh::Ptr& cloud_mesh,
+ const vtkSmartPointer<vtkRenderWindow>& render_window);
+ ~PointsActorItem();
- protected:
- void
- initImpl() override;
+ std::string
+ getItemName() const override
+ {
+ return "Points Actor Item";
+ }
- void
- updateImpl() override;
+protected:
+ void
+ initImpl() override;
- void
- prepareContextMenu(QMenu* menu) const override;
+ void
+ updateImpl() override;
- void
- prepareProperties(ParameterDialog* parameter_dialog) override;
+ void
+ prepareContextMenu(QMenu* menu) const override;
- void
- setProperties() override;
+ void
+ prepareProperties(ParameterDialog* parameter_dialog) override;
- private:
+ void
+ setProperties() override;
- };
- }
-}
+private:
+};
+
+} // namespace modeler
+} // namespace pcl
#include <pcl/apps/modeler/abstract_worker.h>
-namespace pcl
-{
- namespace modeler
- {
- class IntParameter;
- class DoubleParameter;
+namespace pcl {
+namespace modeler {
- class PoissonReconstructionWorker : public AbstractWorker
- {
- public:
- PoissonReconstructionWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent=nullptr);
- ~PoissonReconstructionWorker();
+class IntParameter;
+class DoubleParameter;
- protected:
- std::string
- getName () const override {return ("Poisson Reconstruction");}
+class PoissonReconstructionWorker : public AbstractWorker {
+public:
+ PoissonReconstructionWorker(const QList<CloudMeshItem*>& cloud_mesh_items,
+ QWidget* parent = nullptr);
+ ~PoissonReconstructionWorker();
- void
- initParameters (CloudMeshItem*) override {}
+protected:
+ std::string
+ getName() const override
+ {
+ return "Poisson Reconstruction";
+ }
- void
- setupParameters() override;
+ void
+ initParameters(CloudMeshItem*) override
+ {}
- void
- processImpl(CloudMeshItem* cloud_mesh_item) override;
+ void
+ setupParameters() override;
- private:
- IntParameter* depth_;
- IntParameter* solver_divide_;
- IntParameter* iso_divide_;
- IntParameter* degree_;
- DoubleParameter* scale_;
- DoubleParameter* samples_per_node_;
- };
+ void
+ processImpl(CloudMeshItem* cloud_mesh_item) override;
- }
-}
+private:
+ IntParameter* depth_;
+ IntParameter* solver_divide_;
+ IntParameter* iso_divide_;
+ IntParameter* degree_;
+ DoubleParameter* scale_;
+ DoubleParameter* samples_per_node_;
+};
+
+} // namespace modeler
+} // namespace pcl
#pragma once
-#include <vtkSmartPointer.h>
-
#include <QVTKWidget.h>
+#include <vtkSmartPointer.h>
+
class vtkCubeAxesActor;
-namespace pcl
-{
- namespace modeler
- {
- class RenderWindowItem;
+namespace pcl {
+namespace modeler {
- class RenderWindow : public QVTKWidget
- {
- public:
- RenderWindow(RenderWindowItem* render_window_item, QWidget *parent = nullptr, Qt::WindowFlags flags = nullptr);
- ~RenderWindow();
+class RenderWindowItem;
- QSize
- sizeHint() const override {return {512, 512};}
+class RenderWindow : public QVTKWidget {
+public:
+ RenderWindow(RenderWindowItem* render_window_item,
+ QWidget* parent = nullptr,
+ Qt::WindowFlags flags = nullptr);
+ ~RenderWindow();
- void
- setActive(bool flag);
+ QSize
+ sizeHint() const override
+ {
+ return {512, 512};
+ }
- void
- setTitle(const QString& title);
+ void
+ setActive(bool flag);
- void
- render();
+ void
+ setTitle(const QString& title);
- void
- resetCamera();
+ void
+ render();
- void
- updateAxes();
+ void
+ resetCamera();
- void
- getBackground(double& r, double& g, double& b);
+ void
+ updateAxes();
- void
- setBackground(double r, double g, double b);
+ void
+ getBackground(double& r, double& g, double& b);
- void
- setShowAxes(bool flag);
+ void
+ setBackground(double r, double g, double b);
- protected:
- void
- focusInEvent(QFocusEvent * event) override;
+ void
+ setShowAxes(bool flag);
- private:
- void
- initRenderer();
+protected:
+ void
+ focusInEvent(QFocusEvent* event) override;
- vtkSmartPointer<vtkCubeAxesActor> axes_;
- RenderWindowItem* render_window_item_;
- };
- }
-}
+private:
+ void
+ initRenderer();
+
+ vtkSmartPointer<vtkCubeAxesActor> axes_;
+ RenderWindowItem* render_window_item_;
+};
+
+} // namespace modeler
+} // namespace pcl
- /*
+/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
#pragma once
-#include <QTreeWidgetItem>
-
#include <pcl/apps/modeler/abstract_item.h>
#include <pcl/apps/modeler/cloud_mesh.h>
+#include <QTreeWidgetItem>
-namespace pcl
-{
- namespace modeler
- {
- class RenderWindow;
- class CloudMeshItem;
- class ColorParameter;
- class BoolParameter;
+namespace pcl {
+namespace modeler {
- class RenderWindowItem : public QTreeWidgetItem, public AbstractItem
- {
- public:
- RenderWindowItem(QTreeWidget * parent);
- ~RenderWindowItem();
+class RenderWindow;
+class CloudMeshItem;
+class ColorParameter;
+class BoolParameter;
- inline RenderWindow*
- getRenderWindow()
- {
- return render_window_;
- }
- inline const RenderWindow*
- getRenderWindow() const
- {
- return render_window_;
- }
+class RenderWindowItem : public QTreeWidgetItem, public AbstractItem {
+public:
+ RenderWindowItem(QTreeWidget* parent);
+ ~RenderWindowItem();
- bool
- openPointCloud(const QString& filename);
+ inline RenderWindow*
+ getRenderWindow()
+ {
+ return render_window_;
+ }
+ inline const RenderWindow*
+ getRenderWindow() const
+ {
+ return render_window_;
+ }
+
+ bool
+ openPointCloud(const QString& filename);
+
+ CloudMeshItem*
+ addPointCloud(CloudMesh::PointCloudPtr cloud);
- CloudMeshItem*
- addPointCloud(CloudMesh::PointCloudPtr cloud);
+ std::string
+ getItemName() const override
+ {
+ return "Render Window Item";
+ }
- std::string
- getItemName() const override {return "Render Window Item";}
+protected:
+ void
+ prepareContextMenu(QMenu* menu) const override;
- protected:
- void
- prepareContextMenu(QMenu* menu) const override;
+ void
+ prepareProperties(ParameterDialog* parameter_dialog) override;
- void
- prepareProperties(ParameterDialog* parameter_dialog) override;
+ void
+ setProperties() override;
- void
- setProperties() override;
+private:
+ RenderWindow* render_window_;
+ ColorParameter* background_color_;
+ BoolParameter* show_axes_;
+};
- private:
- RenderWindow* render_window_;
- ColorParameter* background_color_;
- BoolParameter* show_axes_;
- };
- }
-}
+} // namespace modeler
+} // namespace pcl
#include <QTreeWidget>
-namespace pcl
-{
- namespace modeler
- {
- class CloudMeshItem;
- class RenderWindowItem;
+namespace pcl {
+namespace modeler {
- class SceneTree : public QTreeWidget
- {
- Q_OBJECT
+class CloudMeshItem;
+class RenderWindowItem;
- public:
- SceneTree(QWidget * parent = nullptr);
- ~SceneTree();
+class SceneTree : public QTreeWidget {
+ Q_OBJECT
- QSize
- sizeHint() const override;
+public:
+ SceneTree(QWidget* parent = nullptr);
+ ~SceneTree();
- bool
- openPointCloud(const QString& filename);
+ QSize
+ sizeHint() const override;
- bool
- savePointCloud(const QString& filename);
+ bool
+ openPointCloud(const QString& filename);
- void
- selectRenderWindowItem(RenderWindowItem* render_window_item);
+ bool
+ savePointCloud(const QString& filename);
- void
- addTopLevelItem(RenderWindowItem* render_window_item);
+ void
+ selectRenderWindowItem(RenderWindowItem* render_window_item);
- public Q_SLOTS:
- // slots for file menu
- void
- slotOpenPointCloud();
+ void
+ addTopLevelItem(RenderWindowItem* render_window_item);
- void
- slotImportPointCloud();
+public Q_SLOTS:
+ // slots for file menu
+ void
+ slotOpenPointCloud();
- void
- slotSavePointCloud();
+ void
+ slotImportPointCloud();
- void
- slotClosePointCloud();
+ void
+ slotSavePointCloud();
- // slots for edit menu
- void
- slotICPRegistration();
- void
- slotVoxelGridDownsampleFilter();
- void
- slotStatisticalOutlierRemovalFilter();
- void
- slotEstimateNormal();
- void
- slotPoissonReconstruction();
+ void
+ slotClosePointCloud();
- // slots for view menu
- void
- slotCloseRenderWindow();
+ // slots for edit menu
+ void
+ slotICPRegistration();
+ void
+ slotVoxelGridDownsampleFilter();
+ void
+ slotStatisticalOutlierRemovalFilter();
+ void
+ slotEstimateNormal();
+ void
+ slotPoissonReconstruction();
- Q_SIGNALS:
- void
- fileOpened(const QString& filename);
+ // slots for view menu
+ void
+ slotCloseRenderWindow();
- void
- itemInsertedOrRemoved();
+Q_SIGNALS:
+ void
+ fileOpened(const QString& filename);
- protected:
- void
- dropEvent(QDropEvent * event) override;
+ void
+ itemInsertedOrRemoved();
- bool
- dropMimeData(QTreeWidgetItem * parent, int index, const QMimeData * data, Qt::DropAction action) override;
+protected:
+ void
+ dropEvent(QDropEvent* event) override;
- private Q_SLOTS:
- void
- slotUpdateOnSelectionChange(const QItemSelection& selected, const QItemSelection& deselected);
+ bool
+ dropMimeData(QTreeWidgetItem* parent,
+ int index,
+ const QMimeData* data,
+ Qt::DropAction action) override;
- void
- slotUpdateOnInsertOrRemove();
+private Q_SLOTS:
+ void
+ slotUpdateOnSelectionChange(const QItemSelection& selected,
+ const QItemSelection& deselected);
- void
- slotOnItemDoubleClicked(QTreeWidgetItem * item);
+ void
+ slotUpdateOnInsertOrRemove();
- private:
- template <class T> QList<T*>
- selectedTypeItems() const;
+ void
+ slotOnItemDoubleClicked(QTreeWidgetItem* item);
- QList<RenderWindowItem*>
- selectedRenderWindowItems() const;
+private:
+ template <class T>
+ QList<T*>
+ selectedTypeItems() const;
- static void
- closePointCloud(const QList<CloudMeshItem*>& items);
+ QList<RenderWindowItem*>
+ selectedRenderWindowItems() const;
- void
- contextMenuEvent(QContextMenuEvent *event) override;
- };
- }
-}
+ static void
+ closePointCloud(const QList<CloudMeshItem*>& items);
+
+ void
+ contextMenuEvent(QContextMenuEvent* event) override;
+};
+
+} // namespace modeler
+} // namespace pcl
#include <pcl/apps/modeler/impl/scene_tree.hpp>
#include <pcl/apps/modeler/abstract_worker.h>
-namespace pcl
-{
- namespace modeler
- {
- class IntParameter;
- class DoubleParameter;
+namespace pcl {
+namespace modeler {
- class StatisticalOutlierRemovalWorker : public AbstractWorker
- {
- public:
- StatisticalOutlierRemovalWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent=nullptr);
- ~StatisticalOutlierRemovalWorker();
+class IntParameter;
+class DoubleParameter;
- protected:
- std::string
- getName () const override {return ("Statistical Outlier Removal");}
+class StatisticalOutlierRemovalWorker : public AbstractWorker {
+public:
+ StatisticalOutlierRemovalWorker(const QList<CloudMeshItem*>& cloud_mesh_items,
+ QWidget* parent = nullptr);
+ ~StatisticalOutlierRemovalWorker();
- void
- initParameters(CloudMeshItem* cloud_mesh_item) override;
+protected:
+ std::string
+ getName() const override
+ {
+ return "Statistical Outlier Removal";
+ }
- void
- setupParameters() override;
+ void
+ initParameters(CloudMeshItem* cloud_mesh_item) override;
- void
- processImpl(CloudMeshItem* cloud_mesh_item) override;
+ void
+ setupParameters() override;
- private:
- IntParameter* mean_k_;
- DoubleParameter* stddev_mul_thresh_;
+ void
+ processImpl(CloudMeshItem* cloud_mesh_item) override;
- };
+private:
+ IntParameter* mean_k_;
+ DoubleParameter* stddev_mul_thresh_;
+};
- }
-}
+} // namespace modeler
+} // namespace pcl
class vtkIdTypeArray;
-namespace pcl
-{
- namespace modeler
- {
- class SurfaceActorItem : public ChannelActorItem
- {
- public:
- using GeometryHandler = pcl::visualization::PointCloudGeometryHandler<pcl::PointSurfel>;
- using GeometryHandlerPtr = GeometryHandler::Ptr;
- using GeometryHandlerConstPtr = GeometryHandler::ConstPtr;
+namespace pcl {
+namespace modeler {
+
+class SurfaceActorItem : public ChannelActorItem {
+public:
+ using GeometryHandler =
+ pcl::visualization::PointCloudGeometryHandler<pcl::PointSurfel>;
+ using GeometryHandlerPtr = GeometryHandler::Ptr;
+ using GeometryHandlerConstPtr = GeometryHandler::ConstPtr;
+
+ using ColorHandler = pcl::visualization::PointCloudColorHandler<pcl::PointSurfel>;
+ using ColorHandlerPtr = ColorHandler::Ptr;
+ using ColorHandlerConstPtr = ColorHandler::ConstPtr;
- using ColorHandler = pcl::visualization::PointCloudColorHandler<pcl::PointSurfel>;
- using ColorHandlerPtr = ColorHandler::Ptr;
- using ColorHandlerConstPtr = ColorHandler::ConstPtr;
+ SurfaceActorItem(QTreeWidgetItem* parent,
+ const CloudMesh::Ptr& cloud_mesh,
+ const vtkSmartPointer<vtkRenderWindow>& render_window);
+ ~SurfaceActorItem();
- SurfaceActorItem(QTreeWidgetItem* parent,
- const CloudMesh::Ptr& cloud_mesh,
- const vtkSmartPointer<vtkRenderWindow>& render_window);
- ~SurfaceActorItem ();
+ std::string
+ getItemName() const override
+ {
+ return "Points Actor Item";
+ }
- std::string
- getItemName() const override {return "Points Actor Item";}
+protected:
+ void
+ initImpl() override;
- protected:
- void
- initImpl() override;
+ void
+ updateImpl() override;
- void
- updateImpl() override;
+ void
+ prepareContextMenu(QMenu* menu) const override;
- void
- prepareContextMenu(QMenu* menu) const override;
+ void
+ prepareProperties(ParameterDialog* parameter_dialog) override;
- void
- prepareProperties(ParameterDialog* parameter_dialog) override;
+ void
+ setProperties() override;
- void
- setProperties() override;
+private:
+};
- private:
- };
- }
-}
+} // namespace modeler
+} // namespace pcl
#include <QObject>
-namespace pcl
-{
- namespace modeler
- {
- class CloudMeshItem;
- class AbstractWorker;
+namespace pcl {
+namespace modeler {
- class ThreadController : public QObject
- {
- Q_OBJECT
+class CloudMeshItem;
+class AbstractWorker;
- public:
- ThreadController();
- ~ThreadController();
+class ThreadController : public QObject {
+ Q_OBJECT
- bool
- runWorker(AbstractWorker* worker);
+public:
+ ThreadController();
+ ~ThreadController();
- Q_SIGNALS:
- void
- prepared();
+ bool
+ runWorker(AbstractWorker* worker);
- private Q_SLOTS:
- void
- slotOnCloudMeshItemUpdate(CloudMeshItem* cloud_mesh_item);
- };
+Q_SIGNALS:
+ void
+ prepared();
- }
-}
+private Q_SLOTS:
+ void
+ slotOnCloudMeshItemUpdate(CloudMeshItem* cloud_mesh_item);
+};
+
+} // namespace modeler
+} // namespace pcl
#include <pcl/apps/modeler/abstract_worker.h>
-namespace pcl
-{
- namespace modeler
- {
- class DoubleParameter;
+namespace pcl {
+namespace modeler {
- class VoxelGridDownampleWorker : public AbstractWorker
- {
- public:
- VoxelGridDownampleWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent=nullptr);
- ~VoxelGridDownampleWorker();
+class DoubleParameter;
- protected:
- std::string
- getName () const override {return ("Down Sample");}
+class VoxelGridDownampleWorker : public AbstractWorker {
+public:
+ VoxelGridDownampleWorker(const QList<CloudMeshItem*>& cloud_mesh_items,
+ QWidget* parent = nullptr);
+ ~VoxelGridDownampleWorker();
- void
- initParameters(CloudMeshItem* cloud_mesh_item) override;
+protected:
+ std::string
+ getName() const override
+ {
+ return "Down Sample";
+ }
- void
- setupParameters() override;
+ void
+ initParameters(CloudMeshItem* cloud_mesh_item) override;
- void
- processImpl(CloudMeshItem* cloud_mesh_item) override;
+ void
+ setupParameters() override;
- private:
- double x_min_, x_max_;
- double y_min_, y_max_;
- double z_min_, z_max_;
+ void
+ processImpl(CloudMeshItem* cloud_mesh_item) override;
- DoubleParameter* leaf_size_x_;
- DoubleParameter* leaf_size_y_;
- DoubleParameter* leaf_size_z_;
+private:
+ double x_min_, x_max_;
+ double y_min_, y_max_;
+ double z_min_, z_max_;
- };
+ DoubleParameter* leaf_size_x_;
+ DoubleParameter* leaf_size_y_;
+ DoubleParameter* leaf_size_z_;
+};
- }
-}
+} // namespace modeler
+} // namespace pcl
*/
#include <pcl/apps/modeler/abstract_item.h>
-
#include <pcl/apps/modeler/main_window.h>
#include <pcl/apps/modeler/parameter_dialog.h>
-
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::AbstractItem::AbstractItem()
-{
-
-}
+pcl::modeler::AbstractItem::AbstractItem() {}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::AbstractItem::~AbstractItem()
-{
-
-}
+pcl::modeler::AbstractItem::~AbstractItem() {}
//////////////////////////////////////////////////////////////////////////////////////////////
void
QMenu menu(&MainWindow::getInstance());
prepareContextMenu(&menu);
menu.exec(*position);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::AbstractItem::showPropertyEditor()
{
- ParameterDialog* parameter_dialog = new ParameterDialog(getItemName() + " Properties", &MainWindow::getInstance());
+ ParameterDialog* parameter_dialog =
+ new ParameterDialog(getItemName() + " Properties", &MainWindow::getInstance());
prepareProperties(parameter_dialog);
if (parameter_dialog->exec() == QDialog::Accepted)
*/
#include <pcl/apps/modeler/abstract_worker.h>
-
-#include <pcl/apps/modeler/parameter_dialog.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
-
+#include <pcl/apps/modeler/parameter_dialog.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::AbstractWorker::AbstractWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent) :
- cloud_mesh_items_(cloud_mesh_items),
- parameter_dialog_(new ParameterDialog(getName(), parent))
-{
-}
+pcl::modeler::AbstractWorker::AbstractWorker(
+ const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent)
+: cloud_mesh_items_(cloud_mesh_items)
+, parameter_dialog_(new ParameterDialog(getName(), parent))
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::AbstractWorker::~AbstractWorker()
-{
- parameter_dialog_->deleteLater();
-}
+pcl::modeler::AbstractWorker::~AbstractWorker() { parameter_dialog_->deleteLater(); }
//////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::modeler::AbstractWorker::exec()
{
- for (auto &cloud_mesh_item : cloud_mesh_items_)
+ for (auto& cloud_mesh_item : cloud_mesh_items_)
initParameters(cloud_mesh_item);
setupParameters();
- return (parameter_dialog_->exec());
+ return parameter_dialog_->exec();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::AbstractWorker::process()
{
- for (auto &cloud_mesh_item : cloud_mesh_items_)
- {
+ for (auto& cloud_mesh_item : cloud_mesh_items_) {
processImpl(cloud_mesh_item);
}
emit finished();
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
*/
#include <pcl/apps/modeler/channel_actor_item.h>
+#include <pcl/apps/modeler/cloud_mesh.h>
+#include <pcl/point_cloud.h>
#include <vtkCamera.h>
-#include <vtkPolyData.h>
-#include <vtkRenderer.h>
#include <vtkMatrix4x4.h>
+#include <vtkPolyData.h>
#include <vtkRenderWindow.h>
+#include <vtkRenderer.h>
#include <vtkRendererCollection.h>
-#include <pcl/point_cloud.h>
-#include <pcl/apps/modeler/cloud_mesh.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::ChannelActorItem::ChannelActorItem(QTreeWidgetItem* parent,
- const CloudMesh::Ptr& cloud_mesh,
- const vtkSmartPointer<vtkRenderWindow>& render_window,
- const vtkSmartPointer<vtkActor>& actor,
- const std::string& channel_name)
- :QTreeWidgetItem(parent),
- cloud_mesh_(cloud_mesh),
- poly_data_(vtkSmartPointer<vtkPolyData>::New()),
- render_window_(render_window),
- color_scheme_("rgb"),
- actor_(actor)
+pcl::modeler::ChannelActorItem::ChannelActorItem(
+ QTreeWidgetItem* parent,
+ const CloudMesh::Ptr& cloud_mesh,
+ const vtkSmartPointer<vtkRenderWindow>& render_window,
+ const vtkSmartPointer<vtkActor>& actor,
+ const std::string& channel_name)
+: QTreeWidgetItem(parent)
+, cloud_mesh_(cloud_mesh)
+, poly_data_(vtkSmartPointer<vtkPolyData>::New())
+, render_window_(render_window)
+, color_scheme_("rgb")
+, actor_(actor)
{
- setFlags(flags()&(~Qt::ItemIsDragEnabled));
- setFlags(flags()&(~Qt::ItemIsDropEnabled));
+ setFlags(flags() & (~Qt::ItemIsDragEnabled));
+ setFlags(flags() & (~Qt::ItemIsDropEnabled));
setText(0, QString(channel_name.c_str()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::ChannelActorItem::~ChannelActorItem ()
-{
- detachActor();
-}
+pcl::modeler::ChannelActorItem::~ChannelActorItem() { detachActor(); }
//////////////////////////////////////////////////////////////////////////////////////////////
void
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::ChannelActorItem::prepareContextMenu(QMenu*) const
-{
-
-}
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
detachActor();
render_window_ = vtkSmartPointer<vtkRenderWindow>(render_window);
attachActor();
-
- return;
}
*/
#include <pcl/apps/modeler/cloud_mesh.h>
-
-#include <pcl/visualization/point_cloud_handlers.h>
-#include <pcl/filters/filter_indices.h>
#include <pcl/common/transforms.h>
-#include <pcl/PolygonMesh.h>
-#include <pcl/io/pcd_io.h>
+#include <pcl/filters/filter_indices.h>
#include <pcl/io/obj_io.h>
-#include <vtkDataArray.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/point_cloud_handlers.h>
+#include <pcl/PolygonMesh.h>
+
#include <vtkCellArray.h>
+#include <vtkDataArray.h>
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::CloudMesh::CloudMesh()
- :vtk_points_(vtkSmartPointer<vtkPoints>::New()),
- vtk_polygons_(vtkSmartPointer<vtkCellArray>::New())
+: vtk_points_(vtkSmartPointer<vtkPoints>::New())
+, vtk_polygons_(vtkSmartPointer<vtkCellArray>::New())
{
cloud_.reset(new pcl::PointCloud<pcl::PointSurfel>());
- vtk_points_->SetDataTypeToFloat ();
+ vtk_points_->SetDataTypeToFloat();
}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::CloudMesh::CloudMesh(PointCloudPtr cloud)
- :cloud_(std::move(cloud)),
- vtk_points_(vtkSmartPointer<vtkPoints>::New()),
- vtk_polygons_(vtkSmartPointer<vtkCellArray>::New())
+: cloud_(std::move(cloud))
+, vtk_points_(vtkSmartPointer<vtkPoints>::New())
+, vtk_polygons_(vtkSmartPointer<vtkCellArray>::New())
{
- vtk_points_->SetDataTypeToFloat ();
+ vtk_points_->SetDataTypeToFloat();
updateVtkPoints();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::CloudMesh::~CloudMesh ()
-{
-}
+pcl::modeler::CloudMesh::~CloudMesh() {}
//////////////////////////////////////////////////////////////////////////////////////////////
std::vector<std::string>
pcl::modeler::CloudMesh::getAvaiableFieldNames() const
{
// TODO:
- return (std::vector<std::string>());
+ return {};
}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::CloudMesh::open(const std::string& filename)
{
if (pcl::io::loadPCDFile(filename, *cloud_) != 0)
- return (false);
+ return false;
updateVtkPoints();
- return (true);
+ return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::modeler::CloudMesh::save(const std::string& filename) const
{
- if (filename.rfind(".obj") == (filename.length()-4))
- {
+ if (filename.rfind(".obj") == (filename.length() - 4)) {
pcl::PolygonMesh polygon_mesh;
pcl::toPCLPointCloud2(*cloud_, polygon_mesh.cloud);
polygon_mesh.polygons = polygons_;
- return (pcl::io::saveOBJFile(filename, polygon_mesh, true) == 0);
+ return pcl::io::saveOBJFile(filename, polygon_mesh, true) == 0;
}
- return (pcl::io::savePCDFile(filename, *cloud_, true) == 0);
+ return pcl::io::savePCDFile(filename, *cloud_, true) == 0;
}
//////////////////////////////////////////////////////////////////////////////////////////////
bool
-pcl::modeler::CloudMesh::save(const std::vector<const CloudMesh*>& cloud_meshes, const std::string& filename)
+pcl::modeler::CloudMesh::save(const std::vector<const CloudMesh*>& cloud_meshes,
+ const std::string& filename)
{
if (cloud_meshes.empty())
return false;
if (cloud_meshes.size() == 1)
- return (cloud_meshes[0]->save(filename));
+ return cloud_meshes[0]->save(filename);
CloudMesh cloud_mesh;
- for (const auto &mesh : cloud_meshes)
- {
- if (filename.rfind(".obj") == (filename.length()-4))
- {
+ for (const auto& mesh : cloud_meshes) {
+ if (filename.rfind(".obj") == (filename.length() - 4)) {
std::size_t delta = cloud_mesh.cloud_->size();
- for (auto polygon : mesh->polygons_)
- {
- for (unsigned int &vertice : polygon.vertices)
- vertice += static_cast<unsigned int> (delta);
+ for (auto polygon : mesh->polygons_) {
+ for (unsigned int& vertice : polygon.vertices)
+ vertice += static_cast<unsigned int>(delta);
cloud_mesh.polygons_.push_back(polygon);
}
}
*cloud_mesh.cloud_ += *(mesh->cloud_);
}
- return (cloud_mesh.save(filename));
+ return cloud_mesh.save(filename);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::CloudMesh::getColorScalarsFromField(vtkSmartPointer<vtkDataArray> &scalars, const std::string& field) const
+pcl::modeler::CloudMesh::getColorScalarsFromField(
+ vtkSmartPointer<vtkDataArray>& scalars, const std::string& field) const
{
- if (field == "rgb" || field == "rgba")
- {
+ if (field == "rgb" || field == "rgba") {
pcl::visualization::PointCloudColorHandlerRGBField<PointT> color_handler(cloud_);
scalars = color_handler.getColor();
return;
}
- if (field == "random")
- {
+ if (field == "random") {
pcl::visualization::PointCloudColorHandlerRandom<PointT> color_handler(cloud_);
scalars = color_handler.getColor();
return;
}
- pcl::visualization::PointCloudColorHandlerGenericField<PointT> color_handler(cloud_, field);
+ pcl::visualization::PointCloudColorHandlerGenericField<PointT> color_handler(cloud_,
+ field);
scalars = color_handler.getColor();
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::CloudMesh::updateVtkPoints()
{
if (vtk_points_->GetData() == nullptr)
- vtk_points_->SetData(vtkSmartPointer<vtkFloatArray>::New ());
+ vtk_points_->SetData(vtkSmartPointer<vtkFloatArray>::New());
vtkFloatArray* data = dynamic_cast<vtkFloatArray*>(vtk_points_->GetData());
- data->SetNumberOfComponents (3);
+ data->SetNumberOfComponents(3);
// If the dataset has no invalid values, just copy all of them
- if (cloud_->is_dense)
- {
- vtkIdType nr_points = cloud_->points.size ();
- data->SetNumberOfValues(3*nr_points);
-
- for (vtkIdType i = 0; i < nr_points; ++i)
- {
- data->SetValue(i*3+0, cloud_->points[i].x);
- data->SetValue(i*3+1, cloud_->points[i].y);
- data->SetValue(i*3+2, cloud_->points[i].z);
+ if (cloud_->is_dense) {
+ vtkIdType nr_points = cloud_->points.size();
+ data->SetNumberOfValues(3 * nr_points);
+
+ for (vtkIdType i = 0; i < nr_points; ++i) {
+ data->SetValue(i * 3 + 0, cloud_->points[i].x);
+ data->SetValue(i * 3 + 1, cloud_->points[i].y);
+ data->SetValue(i * 3 + 2, cloud_->points[i].z);
}
}
// Need to check for NaNs, Infs, ec
- else
- {
+ else {
pcl::IndicesPtr indices(new std::vector<int>());
pcl::removeNaNFromPointCloud(*cloud_, *indices);
- data->SetNumberOfValues(3*indices->size());
+ data->SetNumberOfValues(3 * indices->size());
- for (vtkIdType i = 0, i_end = indices->size(); i < i_end; ++i)
- {
+ for (vtkIdType i = 0, i_end = indices->size(); i < i_end; ++i) {
vtkIdType idx = (*indices)[i];
- data->SetValue(i*3+0, cloud_->points[idx].x);
- data->SetValue(i*3+1, cloud_->points[idx].y);
- data->SetValue(i*3+2, cloud_->points[idx].z);
+ data->SetValue(i * 3 + 0, cloud_->points[idx].x);
+ data->SetValue(i * 3 + 1, cloud_->points[idx].y);
+ data->SetValue(i * 3 + 2, cloud_->points[idx].z);
}
}
data->Squeeze();
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
{
vtk_polygons_->Reset();
- if (cloud_->is_dense)
- {
- for (const auto &polygon : polygons_)
- {
- vtk_polygons_->InsertNextCell (polygon.vertices.size());
- for (const unsigned int &vertex : polygon.vertices)
- vtk_polygons_->InsertCellPoint (vertex);
+ if (cloud_->is_dense) {
+ for (const auto& polygon : polygons_) {
+ vtk_polygons_->InsertNextCell(polygon.vertices.size());
+ for (const unsigned int& vertex : polygon.vertices)
+ vtk_polygons_->InsertCellPoint(vertex);
}
}
- else
- {
+ else {
pcl::IndicesPtr indices(new std::vector<int>());
pcl::removeNaNFromPointCloud(*cloud_, *indices);
- for (const auto &polygon : polygons_)
- {
- vtk_polygons_->InsertNextCell (polygon.vertices.size());
- for (const unsigned int &vertex : polygon.vertices)
- vtk_polygons_->InsertCellPoint ((*indices)[vertex]);
+ for (const auto& polygon : polygons_) {
+ vtk_polygons_->InsertNextCell(polygon.vertices.size());
+ for (const unsigned int& vertex : polygon.vertices)
+ vtk_polygons_->InsertCellPoint((*indices)[vertex]);
}
}
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::CloudMesh::transform(double tx, double ty, double tz, double rx, double ry, double rz)
+pcl::modeler::CloudMesh::transform(
+ double tx, double ty, double tz, double rx, double ry, double rz)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud_, centroid);
CloudMesh::PointCloud mean_cloud = *cloud_;
pcl::demeanPointCloud(*cloud_, centroid, mean_cloud);
- rx *= M_PI/180;
- ry *= M_PI/180;
- rz *= M_PI/180;
- Eigen::Affine3f affine_transform = pcl::getTransformation (float (tx), float (ty), float (tz), float (rx), float (ry), float (rz));
+ rx *= M_PI / 180;
+ ry *= M_PI / 180;
+ rz *= M_PI / 180;
+ Eigen::Affine3f affine_transform = pcl::getTransformation(
+ float(tx), float(ty), float(tz), float(rx), float(ry), float(rz));
CloudMesh::PointCloud transform_cloud = mean_cloud;
- pcl::transformPointCloudWithNormals(mean_cloud, transform_cloud, affine_transform.matrix());
+ pcl::transformPointCloudWithNormals(
+ mean_cloud, transform_cloud, affine_transform.matrix());
centroid = -centroid;
pcl::demeanPointCloud(transform_cloud, centroid, *cloud_);
-
- return;
}
*
*/
-#include <pcl/apps/modeler/cloud_mesh_item.h>
-#include <pcl/apps/modeler/render_window.h>
-#include <pcl/apps/modeler/render_window_item.h>
-#include <pcl/apps/modeler/points_actor_item.h>
-#include <pcl/apps/modeler/normals_actor_item.h>
-#include <pcl/apps/modeler/surface_actor_item.h>
#include <pcl/apps/modeler/cloud_mesh.h>
+#include <pcl/apps/modeler/cloud_mesh_item.h>
#include <pcl/apps/modeler/main_window.h>
+#include <pcl/apps/modeler/normals_actor_item.h>
#include <pcl/apps/modeler/parameter.h>
#include <pcl/apps/modeler/parameter_dialog.h>
+#include <pcl/apps/modeler/points_actor_item.h>
+#include <pcl/apps/modeler/render_window.h>
+#include <pcl/apps/modeler/render_window_item.h>
+#include <pcl/apps/modeler/surface_actor_item.h>
#include <pcl/common/common.h>
-#include <vtkRenderWindow.h>
+#include <vtkRenderWindow.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::CloudMeshItem::CloudMeshItem (QTreeWidgetItem* parent, const std::string& filename)
- :QTreeWidgetItem(parent),
- filename_(filename),
- cloud_mesh_(new CloudMesh),
- translation_x_(new DoubleParameter("Translation X", "Translation X", 0.0, -1.0, 1.0)),
- translation_y_(new DoubleParameter("Translation Y", "Translation Y", 0.0, -1.0, 1.0)),
- translation_z_(new DoubleParameter("Translation Z", "Translation Z", 0.0, -1.0, 1.0)),
- rotation_x_(new DoubleParameter("Rotation X", "Rotation X", 0.0, -180.0, 180.0)),
- rotation_y_(new DoubleParameter("Rotation Y", "Rotation Y", 0.0, -180.0, 180.0)),
- rotation_z_(new DoubleParameter("Rotation Z", "Rotation Z", 0.0, -180.0, 180.0))
+pcl::modeler::CloudMeshItem::CloudMeshItem(QTreeWidgetItem* parent,
+ const std::string& filename)
+: QTreeWidgetItem(parent)
+, filename_(filename)
+, cloud_mesh_(new CloudMesh)
+, translation_x_(new DoubleParameter("Translation X", "Translation X", 0.0, -1.0, 1.0))
+, translation_y_(new DoubleParameter("Translation Y", "Translation Y", 0.0, -1.0, 1.0))
+, translation_z_(new DoubleParameter("Translation Z", "Translation Z", 0.0, -1.0, 1.0))
+, rotation_x_(new DoubleParameter("Rotation X", "Rotation X", 0.0, -180.0, 180.0))
+, rotation_y_(new DoubleParameter("Rotation Y", "Rotation Y", 0.0, -180.0, 180.0))
+, rotation_z_(new DoubleParameter("Rotation Z", "Rotation Z", 0.0, -180.0, 180.0))
{
- setFlags(flags()&(~Qt::ItemIsDropEnabled));
+ setFlags(flags() & (~Qt::ItemIsDropEnabled));
setText(0, QString(filename.c_str()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::CloudMeshItem::CloudMeshItem (QTreeWidgetItem* parent, CloudMesh::PointCloudPtr cloud)
- :QTreeWidgetItem(parent),
- filename_("unnamed point cloud"),
- cloud_mesh_(new CloudMesh (std::move(cloud))),
- translation_x_(new DoubleParameter("Translation X", "Translation X", 0.0, -1.0, 1.0)),
- translation_y_(new DoubleParameter("Translation Y", "Translation Y", 0.0, -1.0, 1.0)),
- translation_z_(new DoubleParameter("Translation Z", "Translation Z", 0.0, -1.0, 1.0)),
- rotation_x_(new DoubleParameter("Rotation X", "Rotation X", 0.0, -180.0, 180.0)),
- rotation_y_(new DoubleParameter("Rotation Y", "Rotation Y", 0.0, -180.0, 180.0)),
- rotation_z_(new DoubleParameter("Rotation Z", "Rotation Z", 0.0, -180.0, 180.0))
+pcl::modeler::CloudMeshItem::CloudMeshItem(QTreeWidgetItem* parent,
+ CloudMesh::PointCloudPtr cloud)
+: QTreeWidgetItem(parent)
+, filename_("unnamed point cloud")
+, cloud_mesh_(new CloudMesh(std::move(cloud)))
+, translation_x_(new DoubleParameter("Translation X", "Translation X", 0.0, -1.0, 1.0))
+, translation_y_(new DoubleParameter("Translation Y", "Translation Y", 0.0, -1.0, 1.0))
+, translation_z_(new DoubleParameter("Translation Z", "Translation Z", 0.0, -1.0, 1.0))
+, rotation_x_(new DoubleParameter("Rotation X", "Rotation X", 0.0, -180.0, 180.0))
+, rotation_y_(new DoubleParameter("Rotation Y", "Rotation Y", 0.0, -180.0, 180.0))
+, rotation_z_(new DoubleParameter("Rotation Z", "Rotation Z", 0.0, -180.0, 180.0))
{
- setFlags(flags()&(~Qt::ItemIsDropEnabled));
+ setFlags(flags() & (~Qt::ItemIsDropEnabled));
setText(0, QString(filename_.c_str()));
createChannels();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::CloudMeshItem::CloudMeshItem(QTreeWidgetItem* parent, const CloudMeshItem& cloud_mesh_item)
- :QTreeWidgetItem(parent),
- filename_(cloud_mesh_item.filename_),
- cloud_mesh_(cloud_mesh_item.cloud_mesh_),
- translation_x_(new DoubleParameter("Translation X", "Translation X", 0.0, -1.0, 1.0)),
- translation_y_(new DoubleParameter("Translation Y", "Translation Y", 0.0, -1.0, 1.0)),
- translation_z_(new DoubleParameter("Translation Z", "Translation Z", 0.0, -1.0, 1.0)),
- rotation_x_(new DoubleParameter("Rotation X", "Rotation X", 0.0, -180.0, 180.0)),
- rotation_y_(new DoubleParameter("Rotation Y", "Rotation Y", 0.0, -180.0, 180.0)),
- rotation_z_(new DoubleParameter("Rotation Z", "Rotation Z", 0.0, -180.0, 180.0))
+pcl::modeler::CloudMeshItem::CloudMeshItem(QTreeWidgetItem* parent,
+ const CloudMeshItem& cloud_mesh_item)
+: QTreeWidgetItem(parent)
+, filename_(cloud_mesh_item.filename_)
+, cloud_mesh_(cloud_mesh_item.cloud_mesh_)
+, translation_x_(new DoubleParameter("Translation X", "Translation X", 0.0, -1.0, 1.0))
+, translation_y_(new DoubleParameter("Translation Y", "Translation Y", 0.0, -1.0, 1.0))
+, translation_z_(new DoubleParameter("Translation Z", "Translation Z", 0.0, -1.0, 1.0))
+, rotation_x_(new DoubleParameter("Rotation X", "Rotation X", 0.0, -180.0, 180.0))
+, rotation_y_(new DoubleParameter("Rotation Y", "Rotation Y", 0.0, -180.0, 180.0))
+, rotation_z_(new DoubleParameter("Rotation Z", "Rotation Z", 0.0, -180.0, 180.0))
{
- setFlags(flags()&(~Qt::ItemIsDropEnabled));
+ setFlags(flags() & (~Qt::ItemIsDropEnabled));
setText(0, QString(filename_.c_str()));
createChannels();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::CloudMeshItem::~CloudMeshItem ()
-{
-}
+pcl::modeler::CloudMeshItem::~CloudMeshItem() {}
//////////////////////////////////////////////////////////////////////////////////////////////
bool
-pcl::modeler::CloudMeshItem::savePointCloud(const QList<CloudMeshItem*>& items, const QString& filename)
+pcl::modeler::CloudMeshItem::savePointCloud(const QList<CloudMeshItem*>& items,
+ const QString& filename)
{
if (items.size() == 1)
- return (items.first()->getCloudMesh()->save(filename.toStdString()));
+ return items.first()->getCloudMesh()->save(filename.toStdString());
std::vector<const CloudMesh*> cloud_meshes;
- for (const auto &item : items)
- {
+ for (const auto& item : items) {
cloud_meshes.push_back(item->getCloudMesh().get());
}
- return (CloudMesh::save(cloud_meshes, filename.toStdString()));
+ return CloudMesh::save(cloud_meshes, filename.toStdString());
}
//////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::modeler::CloudMeshItem::open()
{
- if(!cloud_mesh_->open(filename_))
- return (false);
+ if (!cloud_mesh_->open(filename_))
+ return false;
createChannels();
treeWidget()->expandItem(this);
- return (true);
+ return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::CloudMeshItem::createChannels()
{
RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(parent());
- addChild(new PointsActorItem(this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
- addChild(new NormalsActorItem(this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
- addChild(new SurfaceActorItem(this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
- for (int i = 0, i_end = childCount(); i < i_end; ++ i)
- {
+ addChild(new PointsActorItem(
+ this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
+ addChild(new NormalsActorItem(
+ this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
+ addChild(new SurfaceActorItem(
+ this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
+ for (int i = 0, i_end = childCount(); i < i_end; ++i) {
ChannelActorItem* child_item = dynamic_cast<ChannelActorItem*>(child(i));
child_item->init();
}
render_window_item->getRenderWindow()->updateAxes();
render_window_item->getRenderWindow()->resetCamera();
-
- return;
}
-
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::CloudMeshItem::updateChannels()
cloud_mesh_->updateVtkPoints();
cloud_mesh_->updateVtkPolygons();
- for (int i = 0, i_end = childCount(); i < i_end; ++ i)
- {
+ for (int i = 0, i_end = childCount(); i < i_end; ++i) {
ChannelActorItem* child_item = dynamic_cast<ChannelActorItem*>(child(i));
child_item->update();
}
RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(parent());
render_window_item->getRenderWindow()->updateAxes();
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
double x_range = max_pt.x() - min_pt.x();
double y_range = max_pt.y() - min_pt.y();
double z_range = max_pt.z() - min_pt.z();
- translation_x_->setLow(-x_range/2);
- translation_x_->setHigh(x_range/2);
- translation_x_->setStep(x_range/1000);
- translation_y_->setLow(-y_range/2);
- translation_y_->setHigh(y_range/2);
- translation_y_->setStep(y_range/1000);
- translation_z_->setLow(-z_range/2);
- translation_z_->setHigh(z_range/2);
- translation_z_->setStep(z_range/1000);
+ translation_x_->setLow(-x_range / 2);
+ translation_x_->setHigh(x_range / 2);
+ translation_x_->setStep(x_range / 1000);
+ translation_y_->setLow(-y_range / 2);
+ translation_y_->setHigh(y_range / 2);
+ translation_y_->setStep(y_range / 1000);
+ translation_z_->setLow(-z_range / 2);
+ translation_z_->setHigh(z_range / 2);
+ translation_z_->setStep(z_range / 1000);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::CloudMeshItem::setProperties()
{
- cloud_mesh_->transform(*translation_x_, *translation_y_, *translation_z_,
- *rotation_x_, *rotation_y_, *rotation_z_);
+ cloud_mesh_->transform(*translation_x_,
+ *translation_y_,
+ *translation_z_,
+ *rotation_x_,
+ *rotation_y_,
+ *rotation_z_);
updateChannels();
}
pcl::modeler::CloudMeshItem::updateRenderWindow()
{
RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(parent());
- for (int i = 0, i_end = childCount(); i < i_end; ++ i)
- {
+ for (int i = 0, i_end = childCount(); i < i_end; ++i) {
ChannelActorItem* child_item = dynamic_cast<ChannelActorItem*>(child(i));
- child_item->switchRenderWindow(render_window_item->getRenderWindow()->GetRenderWindow());
+ child_item->switchRenderWindow(
+ render_window_item->getRenderWindow()->GetRenderWindow());
}
render_window_item->getRenderWindow()->updateAxes();
render_window_item->getRenderWindow()->resetCamera();
-
- return;
}
*
*/
-#include <pcl/apps/modeler/cloud_mesh_item_updater.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
-
+#include <pcl/apps/modeler/cloud_mesh_item_updater.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::CloudMeshItemUpdater::CloudMeshItemUpdater (CloudMeshItem* cloud_mesh_item)
- :cloud_mesh_item_(cloud_mesh_item)
-{
-}
+pcl::modeler::CloudMeshItemUpdater::CloudMeshItemUpdater(CloudMeshItem* cloud_mesh_item)
+: cloud_mesh_item_(cloud_mesh_item)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::CloudMeshItemUpdater::~CloudMeshItemUpdater ()
-{
-
-}
+pcl::modeler::CloudMeshItemUpdater::~CloudMeshItemUpdater() {}
//////////////////////////////////////////////////////////////////////////////////////////////
void
*/
#include <pcl/apps/modeler/dock_widget.h>
-#include <pcl/apps/modeler/render_window.h>
#include <pcl/apps/modeler/main_window.h>
-
+#include <pcl/apps/modeler/render_window.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::DockWidget::DockWidget(const QString &title, QWidget *parent, Qt::WindowFlags flags) :
- QDockWidget(title, parent, flags)
+pcl::modeler::DockWidget::DockWidget(const QString& title,
+ QWidget* parent,
+ Qt::WindowFlags flags)
+: QDockWidget(title, parent, flags)
{
setStyleSheet("QDockWidget::title {text-align: center;}");
setFocusPolicy(Qt::StrongFocus);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::DockWidget::DockWidget(QWidget *parent, Qt::WindowFlags flags) :
- QDockWidget(parent, flags)
+pcl::modeler::DockWidget::DockWidget(QWidget* parent, Qt::WindowFlags flags)
+: QDockWidget(parent, flags)
{
setStyleSheet("QDockWidget::title {text-align: center;}");
setFocusPolicy(Qt::StrongFocus);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::DockWidget::~DockWidget()
-{
-}
+pcl::modeler::DockWidget::~DockWidget() {}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::DockWidget::focusInEvent ( QFocusEvent * event )
+pcl::modeler::DockWidget::focusInEvent(QFocusEvent* event)
{
QDockWidget::focusInEvent(event);
}
void
pcl::modeler::DockWidget::setFocusBasedStyle(bool focused)
{
- if (focused)
- {
+ if (focused) {
setStyleSheet("QDockWidget::title {text-align: center; background: #87CEFA;}");
}
- else
- {
+ else {
setStyleSheet("QDockWidget::title {text-align: center;}");
}
-
- return;
}
*
*/
-#include <pcl/apps/modeler/icp_registration_worker.h>
-#include <pcl/apps/modeler/parameter_dialog.h>
-#include <pcl/apps/modeler/parameter.h>
#include <pcl/apps/modeler/cloud_mesh.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
-#include <pcl/registration/icp.h>
+#include <pcl/apps/modeler/icp_registration_worker.h>
+#include <pcl/apps/modeler/parameter.h>
+#include <pcl/apps/modeler/parameter_dialog.h>
#include <pcl/common/common.h>
+#include <pcl/registration/icp.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::ICPRegistrationWorker::ICPRegistrationWorker(CloudMesh::PointCloudPtr cloud, const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent)
- : AbstractWorker(cloud_mesh_items, parent),
- cloud_(std::move(cloud)),
- x_min_(std::numeric_limits<double>::max()), x_max_(std::numeric_limits<double>::min()),
- y_min_(std::numeric_limits<double>::max()), y_max_(std::numeric_limits<double>::min()),
- z_min_(std::numeric_limits<double>::max()), z_max_(std::numeric_limits<double>::min()),
- max_correspondence_distance_(nullptr),
- max_iterations_(nullptr),
- transformation_epsilon_(nullptr),
- euclidean_fitness_epsilon_(nullptr)
-{
-
-}
+pcl::modeler::ICPRegistrationWorker::ICPRegistrationWorker(
+ CloudMesh::PointCloudPtr cloud,
+ const QList<CloudMeshItem*>& cloud_mesh_items,
+ QWidget* parent)
+: AbstractWorker(cloud_mesh_items, parent)
+, cloud_(std::move(cloud))
+, x_min_(std::numeric_limits<double>::max())
+, x_max_(std::numeric_limits<double>::min())
+, y_min_(std::numeric_limits<double>::max())
+, y_max_(std::numeric_limits<double>::min())
+, z_min_(std::numeric_limits<double>::max())
+, z_max_(std::numeric_limits<double>::min())
+, max_correspondence_distance_(nullptr)
+, max_iterations_(nullptr)
+, transformation_epsilon_(nullptr)
+, euclidean_fitness_epsilon_(nullptr)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::ICPRegistrationWorker::~ICPRegistrationWorker()
-{
-}
+pcl::modeler::ICPRegistrationWorker::~ICPRegistrationWorker() {}
//////////////////////////////////////////////////////////////////////////////////////////////
void
z_min_ = std::min(double(min_pt.z()), z_min_);
z_max_ = std::max(double(max_pt.z()), z_max_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
double z_range = z_max_ - z_min_;
double range_max = std::max(x_range, std::max(y_range, z_range));
- double max_correspondence_distance = range_max/2;
- double step = range_max/1000;
-
- max_correspondence_distance_ = new DoubleParameter("Max Correspondence Distance",
- "If the distance is larger than this threshold, the points will be ignored in the alignment process.", max_correspondence_distance, 0, x_max_-x_min_, step);
-
- max_iterations_ = new IntParameter("Max Iterations",
- "Set the maximum number of iterations the internal optimization should run for.", 10, 0, 256);
-
- double transformation_epsilon = range_max/2;
- transformation_epsilon_ = new DoubleParameter("Transformation Epsilon",
- "Maximum allowable difference between two consecutive transformations.", 0.0, 0, transformation_epsilon, step);
-
- double euclidean_fitness_epsilon = range_max/2;
- euclidean_fitness_epsilon_ = new DoubleParameter("Euclidean Fitness Epsilon",
- "Maximum allowed Euclidean error between two consecutive steps in the ICP loop.", 0.0, 0, euclidean_fitness_epsilon, step);
+ double max_correspondence_distance = range_max / 2;
+ double step = range_max / 1000;
+
+ // clang-format off
+ max_correspondence_distance_ =
+ new DoubleParameter("Max Correspondence Distance",
+ "If the distance is larger than this threshold, the points "
+ "will be ignored in the alignment process.",
+ max_correspondence_distance, 0, x_max_ - x_min_, step);
+
+ max_iterations_ = new IntParameter(
+ "Max Iterations",
+ "Set the maximum number of iterations the internal optimization should run for.",
+ 10, 0, 256);
+
+ double transformation_epsilon = range_max / 2;
+ transformation_epsilon_ = new DoubleParameter(
+ "Transformation Epsilon",
+ "Maximum allowable difference between two consecutive transformations.",
+ 0.0, 0, transformation_epsilon, step);
+
+ double euclidean_fitness_epsilon = range_max / 2;
+ euclidean_fitness_epsilon_ = new DoubleParameter(
+ "Euclidean Fitness Epsilon",
+ "Maximum allowed Euclidean error between two consecutive steps in the ICP loop.",
+ 0.0, 0, euclidean_fitness_epsilon, step);
+ // clang-format on
parameter_dialog_->addParameter(max_correspondence_distance_);
parameter_dialog_->addParameter(max_iterations_);
parameter_dialog_->addParameter(transformation_epsilon_);
parameter_dialog_->addParameter(euclidean_fitness_epsilon_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::ICPRegistrationWorker::processImpl(CloudMeshItem* cloud_mesh_item)
{
- if (cloud_->empty())
- {
+ if (cloud_->empty()) {
*cloud_ = *(cloud_mesh_item->getCloudMesh()->getCloud());
return;
}
pcl::IterativeClosestPoint<CloudMesh::PointT, CloudMesh::PointT> icp;
- // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
- icp.setMaxCorrespondenceDistance (*max_correspondence_distance_);
+ // Set the max correspondence distance to 5cm (e.g., correspondences with higher
+ // distances will be ignored)
+ icp.setMaxCorrespondenceDistance(*max_correspondence_distance_);
// Set the maximum number of iterations (criterion 1)
- icp.setMaximumIterations (*max_iterations_);
+ icp.setMaximumIterations(*max_iterations_);
// Set the transformation epsilon (criterion 2)
- icp.setTransformationEpsilon (*transformation_epsilon_);
+ icp.setTransformationEpsilon(*transformation_epsilon_);
// Set the euclidean distance difference epsilon (criterion 3)
- icp.setEuclideanFitnessEpsilon (*euclidean_fitness_epsilon_);
+ icp.setEuclideanFitnessEpsilon(*euclidean_fitness_epsilon_);
icp.setInputSource(cloud_mesh_item->getCloudMesh()->getCloud());
icp.setInputTarget(cloud_);
icp.align(result);
result.sensor_origin_ = cloud_mesh_item->getCloudMesh()->getCloud()->sensor_origin_;
- result.sensor_orientation_ = cloud_mesh_item->getCloudMesh()->getCloud()->sensor_orientation_;
+ result.sensor_orientation_ =
+ cloud_mesh_item->getCloudMesh()->getCloud()->sensor_orientation_;
*(cloud_mesh_item->getCloudMesh()->getCloud()) = result;
*cloud_ += result;
-
- return;
}
#include <pcl/apps/modeler/main_window.h>
-extern int qInitResources_resources();
+extern int
+qInitResources_resources();
-int main( int argc, char** argv )
+int
+main(int argc, char** argv)
{
- QApplication app( argc, argv );
+ QApplication app(argc, argv);
qInitResources_resources();
*
*/
-#include <pcl/apps/modeler/main_window.h>
-
-#include <pcl/apps/modeler/scene_tree.h>
#include <pcl/apps/modeler/dock_widget.h>
+#include <pcl/apps/modeler/main_window.h>
#include <pcl/apps/modeler/render_window.h>
#include <pcl/apps/modeler/render_window_item.h>
+#include <pcl/apps/modeler/scene_tree.h>
#include <QFileInfo>
#include <QSettings>
+
#include <vtkActor.h>
#include <vtkRenderer.h>
-
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::MainWindow::MainWindow()
- : ui_(new Ui::MainWindow)
+pcl::modeler::MainWindow::MainWindow() : ui_(new Ui::MainWindow)
{
ui_->setupUi(this);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::MainWindow::~MainWindow()
-{
- saveGlobalSettings();
-}
+pcl::modeler::MainWindow::~MainWindow() { saveGlobalSettings(); }
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::connectFileMenuActions()
{
- connect(ui_->actionOpenPointCloud, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotOpenPointCloud()));
- connect(ui_->actionImportPointCloud, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotImportPointCloud()));
- connect(ui_->actionSavePointCloud, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotSavePointCloud()));
- connect(ui_->actionClosePointCloud, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotClosePointCloud()));
- connect(ui_->scene_tree_, SIGNAL(fileOpened(QString)), this, SLOT(slotUpdateRecentFile(QString)));
+ connect(ui_->actionOpenPointCloud,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotOpenPointCloud()));
+ connect(ui_->actionImportPointCloud,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotImportPointCloud()));
+ connect(ui_->actionSavePointCloud,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotSavePointCloud()));
+ connect(ui_->actionClosePointCloud,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotClosePointCloud()));
+ connect(ui_->scene_tree_,
+ SIGNAL(fileOpened(QString)),
+ this,
+ SLOT(slotUpdateRecentFile(QString)));
createRecentPointCloudActions();
connect(ui_->actionOpenProject, SIGNAL(triggered()), this, SLOT(slotOpenProject()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::connectViewMenuActions()
{
- connect(ui_->actionCreateRenderWindow, SIGNAL(triggered()), this, SLOT(slotCreateRenderWindow()));
- connect(ui_->actionCloseRenderWindow, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotCloseRenderWindow()));
+ connect(ui_->actionCreateRenderWindow,
+ SIGNAL(triggered()),
+ this,
+ SLOT(slotCreateRenderWindow()));
+ connect(ui_->actionCloseRenderWindow,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotCloseRenderWindow()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::connectEditMenuActions()
{
- connect(ui_->actionICPRegistration, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotICPRegistration()));
- connect(ui_->actionVoxelGridDownsample, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotVoxelGridDownsampleFilter()));
- connect(ui_->actionStatisticalOutlierRemoval, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotStatisticalOutlierRemovalFilter()));
- connect(ui_->actionEstimateNormals, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotEstimateNormal()));
- connect(ui_->actionPoissonReconstruction, SIGNAL(triggered()), ui_->scene_tree_, SLOT(slotPoissonReconstruction()));
+ connect(ui_->actionICPRegistration,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotICPRegistration()));
+ connect(ui_->actionVoxelGridDownsample,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotVoxelGridDownsampleFilter()));
+ connect(ui_->actionStatisticalOutlierRemoval,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotStatisticalOutlierRemovalFilter()));
+ connect(ui_->actionEstimateNormals,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotEstimateNormal()));
+ connect(ui_->actionPoissonReconstruction,
+ SIGNAL(triggered()),
+ ui_->scene_tree_,
+ SLOT(slotPoissonReconstruction()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::slotOpenProject()
-{
-
-}
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::slotSaveProject()
-{
-
-}
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::slotCloseProject()
-{
-
-}
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::MainWindow::slotExit() {
- saveGlobalSettings();
- qApp->exit();
+pcl::modeler::MainWindow::slotExit()
+{
+ saveGlobalSettings();
+ qApp->exit();
}
//////////////////////////////////////////////////////////////////////////////////////////////
ui_->scene_tree_->addTopLevelItem(render_window_item);
// add the toggle action to view menu
- QList<QAction *> actions = ui_->menuView->actions();
- ui_->menuView->insertAction(actions[actions.size()-2], dock_widget->toggleViewAction());
+ QList<QAction*> actions = ui_->menuView->actions();
+ ui_->menuView->insertAction(actions[actions.size() - 2],
+ dock_widget->toggleViewAction());
return render_window_item;
}
pcl::modeler::MainWindow::slotCreateRenderWindow()
{
createRenderWindow();
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::slotOpenRecentPointCloud()
{
QAction* action = qobject_cast<QAction*>(sender());
if (action)
ui_->scene_tree_->openPointCloud(action->data().toString());
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::slotOpenRecentProject()
{
QAction* action = qobject_cast<QAction*>(sender());
if (action)
openProjectImpl(action->data().toString());
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::createRecentPointCloudActions()
{
- for (std::size_t i = 0; i < MAX_RECENT_NUMBER; ++ i)
- {
+ for (std::size_t i = 0; i < MAX_RECENT_NUMBER; ++i) {
recent_pointcloud_actions_.push_back(std::shared_ptr<QAction>(new QAction(this)));
ui_->menuRecentPointClouds->addAction(recent_pointcloud_actions_[i].get());
recent_pointcloud_actions_[i]->setVisible(false);
- connect(recent_pointcloud_actions_[i].get(), SIGNAL(triggered()), this, SLOT(slotOpenRecentPointCloud()));
+ connect(recent_pointcloud_actions_[i].get(),
+ SIGNAL(triggered()),
+ this,
+ SLOT(slotOpenRecentPointCloud()));
}
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::updateRecentPointCloudActions()
{
updateRecentActions(recent_pointcloud_actions_, recent_files_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::createRecentProjectActions()
{
- for (std::size_t i = 0; i < MAX_RECENT_NUMBER; ++ i)
- {
+ for (std::size_t i = 0; i < MAX_RECENT_NUMBER; ++i) {
recent_project_actions_.push_back(std::shared_ptr<QAction>(new QAction(this)));
ui_->menuRecentPointClouds->addAction(recent_project_actions_[i].get());
recent_project_actions_[i]->setVisible(false);
- connect(recent_project_actions_[i].get(), SIGNAL(triggered()), this, SLOT(slotOpenRecentProject()));
+ connect(recent_project_actions_[i].get(),
+ SIGNAL(triggered()),
+ this,
+ SLOT(slotOpenRecentProject()));
}
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::updateRecentProjectActions()
{
updateRecentActions(recent_project_actions_, recent_projects_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-bool
-pcl::modeler::MainWindow::openProjectImpl (const QString&)
+bool
+pcl::modeler::MainWindow::openProjectImpl(const QString&)
{
- return (true);
+ return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::modeler::MainWindow::updateRecentActions (std::vector<std::shared_ptr<QAction>>& recent_actions, QStringList& recent_items)
+void
+pcl::modeler::MainWindow::updateRecentActions(
+ std::vector<std::shared_ptr<QAction>>& recent_actions, QStringList& recent_items)
{
- QMutableStringListIterator recent_items_it (recent_items);
- while (recent_items_it.hasNext ())
- {
- if (!QFile::exists (recent_items_it.next ()))
- recent_items_it.remove ();
+ QMutableStringListIterator recent_items_it(recent_items);
+ while (recent_items_it.hasNext()) {
+ if (!QFile::exists(recent_items_it.next()))
+ recent_items_it.remove();
}
- recent_items.removeDuplicates ();
- int recent_number = std::min (int (MAX_RECENT_NUMBER), recent_items.size ());
- for (int i = 0; i < recent_number; ++ i)
- {
- QString text = tr ("%1 %2").arg (i + 1).arg (recent_items[i]);
- recent_actions[i]->setText (text);
- recent_actions[i]->setData (recent_items[i]);
- recent_actions[i]->setVisible (true);
+ recent_items.removeDuplicates();
+ int recent_number = std::min(int(MAX_RECENT_NUMBER), recent_items.size());
+ for (int i = 0; i < recent_number; ++i) {
+ QString text = tr("%1 %2").arg(i + 1).arg(recent_items[i]);
+ recent_actions[i]->setText(text);
+ recent_actions[i]->setData(recent_items[i]);
+ recent_actions[i]->setVisible(true);
}
- for (std::size_t i = recent_number, i_end = recent_actions.size (); i < i_end; ++ i)
- recent_actions[i]->setVisible (false);
+ for (std::size_t i = recent_number, i_end = recent_actions.size(); i < i_end; ++i)
+ recent_actions[i]->setVisible(false);
- while (recent_items.size () > recent_number)
- recent_items.pop_back ();
+ while (recent_items.size() > recent_number)
+ recent_items.pop_back();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-QString
+QString
pcl::modeler::MainWindow::getRecentFolder()
{
QString recent_filename;
if (!recent_filename.isEmpty())
return QFileInfo(recent_filename).path();
- return (QString("."));
+ return ".";
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::loadGlobalSettings()
{
QSettings global_settings("PCL", "Modeler");
recent_projects_ = global_settings.value("recent_projects").toStringList();
updateRecentProjectActions();
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::saveGlobalSettings()
{
QSettings global_settings("PCL", "Modeler");
global_settings.setValue("recent_pointclouds", recent_files_);
global_settings.setValue("recent_projects", recent_projects_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::slotOnWorkerStarted()
{
statusBar()->showMessage(QString("Working thread running..."));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::MainWindow::slotOnWorkerFinished()
{
statusBar()->showMessage(QString("Working thread finished..."));
*
*/
-#include <pcl/apps/modeler/normal_estimation_worker.h>
-#include <pcl/apps/modeler/parameter_dialog.h>
-#include <pcl/apps/modeler/parameter.h>
#include <pcl/apps/modeler/cloud_mesh.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
-#include <pcl/filters/filter_indices.h>
+#include <pcl/apps/modeler/normal_estimation_worker.h>
+#include <pcl/apps/modeler/parameter.h>
+#include <pcl/apps/modeler/parameter_dialog.h>
+#include <pcl/common/common.h>
#include <pcl/features/normal_3d.h>
+#include <pcl/filters/filter_indices.h>
#include <pcl/filters/voxel_grid.h>
-#include <pcl/common/common.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::NormalEstimationWorker::NormalEstimationWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent) :
- AbstractWorker(cloud_mesh_items, parent),
- x_min_(std::numeric_limits<double>::max()), x_max_(std::numeric_limits<double>::min()),
- y_min_(std::numeric_limits<double>::max()), y_max_(std::numeric_limits<double>::min()),
- z_min_(std::numeric_limits<double>::max()), z_max_(std::numeric_limits<double>::min()),
- search_radius_(nullptr)
-{
-
-}
+pcl::modeler::NormalEstimationWorker::NormalEstimationWorker(
+ const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent)
+: AbstractWorker(cloud_mesh_items, parent)
+, x_min_(std::numeric_limits<double>::max())
+, x_max_(std::numeric_limits<double>::min())
+, y_min_(std::numeric_limits<double>::max())
+, y_max_(std::numeric_limits<double>::min())
+, z_min_(std::numeric_limits<double>::max())
+, z_max_(std::numeric_limits<double>::min())
+, search_radius_(nullptr)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::NormalEstimationWorker::~NormalEstimationWorker()
z_min_ = std::min(double(min_pt.z()), z_min_);
z_max_ = std::max(double(max_pt.z()), z_max_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
double z_range = z_max_ - z_min_;
double range_max = std::max(x_range, std::max(y_range, z_range));
- double radius = range_max/100;
- double step = range_max/1000;
+ double radius = range_max / 100;
+ double step = range_max / 1000;
- search_radius_ = new DoubleParameter("Search Radius",
- "The sphere radius that is to be used for determining the nearest neighbors", radius, 0, x_max_-x_min_, step);
+ // clang-format off
+ search_radius_ = new DoubleParameter(
+ "Search Radius",
+ "The sphere radius that is to be used for determining the nearest neighbors",
+ radius, 0, x_max_ - x_min_, step);
+ // clang-format on
parameter_dialog_->addParameter(search_radius_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
normal_estimator.setIndices(indices);
// Create an empty kdtree representation, and pass it to the normal estimation object.
- // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
- pcl::search::KdTree<pcl::PointSurfel>::Ptr tree (new pcl::search::KdTree<pcl::PointSurfel> ());
- normal_estimator.setSearchMethod (tree);
+ // Its content will be filled inside the object, based on the given input dataset (as
+ // no other search surface is given).
+ pcl::search::KdTree<pcl::PointSurfel>::Ptr tree(
+ new pcl::search::KdTree<pcl::PointSurfel>());
+ normal_estimator.setSearchMethod(tree);
// Use all neighbors in a sphere of the search radius
- normal_estimator.setRadiusSearch (*search_radius_);
+ normal_estimator.setRadiusSearch(*search_radius_);
pcl::PointCloud<pcl::PointNormal> normals;
normal_estimator.compute(normals);
- for (std::size_t i = 0, i_end = indices->size(); i < i_end; ++ i)
- {
+ for (std::size_t i = 0, i_end = indices->size(); i < i_end; ++i) {
std::size_t dest = (*indices)[i];
cloud->points[dest].normal_x = normals.points[i].normal_x;
cloud->points[dest].normal_y = normals.points[i].normal_y;
cloud->points[dest].normal_z = normals.points[i].normal_z;
}
-
- return;
}
*
*/
-#include <pcl/apps/modeler/normals_actor_item.h>
-
#include <pcl/apps/modeler/cloud_mesh.h>
+#include <pcl/apps/modeler/normals_actor_item.h>
#include <pcl/filters/filter_indices.h>
-#include <vtkLODActor.h>
-#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkDataSetMapper.h>
+#include <vtkLODActor.h>
#include <vtkPointData.h>
+#include <vtkPolyData.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::NormalsActorItem::NormalsActorItem(QTreeWidgetItem* parent,
- const CloudMesh::Ptr& cloud_mesh,
- const vtkSmartPointer<vtkRenderWindow>& render_window)
- :ChannelActorItem(parent, cloud_mesh, render_window, vtkSmartPointer<vtkLODActor>::New(), "Normals"),
- level_(10), scale_(0.1)
-{
-}
+pcl::modeler::NormalsActorItem::NormalsActorItem(
+ QTreeWidgetItem* parent,
+ const CloudMesh::Ptr& cloud_mesh,
+ const vtkSmartPointer<vtkRenderWindow>& render_window)
+: ChannelActorItem(
+ parent, cloud_mesh, render_window, vtkSmartPointer<vtkLODActor>::New(), "Normals")
+, level_(10)
+, scale_(0.1)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::NormalsActorItem::~NormalsActorItem ()
-{
-
-}
+pcl::modeler::NormalsActorItem::~NormalsActorItem() {}
//////////////////////////////////////////////////////////////////////////////////////////////
void
return;
if (points->GetData() == nullptr)
- points->SetData(vtkSmartPointer<vtkFloatArray>::New ());
+ points->SetData(vtkSmartPointer<vtkFloatArray>::New());
vtkFloatArray* data = dynamic_cast<vtkFloatArray*>(points->GetData());
- data->SetNumberOfComponents (3);
-
- if (cloud->is_dense)
- {
- vtkIdType nr_normals = static_cast<vtkIdType> ((cloud->points.size () - 1) / level_ + 1);
- data->SetNumberOfValues(2*3*nr_normals);
- for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = static_cast<vtkIdType> (j * level_))
- {
+ data->SetNumberOfComponents(3);
+
+ if (cloud->is_dense) {
+ vtkIdType nr_normals =
+ static_cast<vtkIdType>((cloud->points.size() - 1) / level_ + 1);
+ data->SetNumberOfValues(2 * 3 * nr_normals);
+ for (vtkIdType i = 0, j = 0; j < nr_normals;
+ j++, i = static_cast<vtkIdType>(j * level_)) {
const CloudMesh::PointT& p = cloud->points[i];
- data->SetValue(2*j*3 + 0, p.x);
- data->SetValue(2*j*3 + 1, p.y);
- data->SetValue(2*j*3 + 2, p.z);
- data->SetValue(2*j*3 + 3, float (p.x + p.normal_x*scale_));
- data->SetValue(2*j*3 + 4, float (p.y + p.normal_y*scale_));
- data->SetValue(2*j*3 + 5, float (p.z + p.normal_z*scale_));
+ data->SetValue(2 * j * 3 + 0, p.x);
+ data->SetValue(2 * j * 3 + 1, p.y);
+ data->SetValue(2 * j * 3 + 2, p.z);
+ data->SetValue(2 * j * 3 + 3, float(p.x + p.normal_x * scale_));
+ data->SetValue(2 * j * 3 + 4, float(p.y + p.normal_y * scale_));
+ data->SetValue(2 * j * 3 + 5, float(p.z + p.normal_z * scale_));
lines->InsertNextCell(2);
- lines->InsertCellPoint(2*j);
- lines->InsertCellPoint(2*j + 1);
+ lines->InsertCellPoint(2 * j);
+ lines->InsertCellPoint(2 * j + 1);
}
}
- else
- {
+ else {
pcl::IndicesPtr indices(new std::vector<int>());
pcl::removeNaNFromPointCloud(*cloud, *indices);
- vtkIdType nr_normals = static_cast<vtkIdType> ((indices->size () - 1) / level_ + 1);
- data->SetNumberOfValues(2*3*nr_normals);
- for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = static_cast<vtkIdType> (j * level_))
- {
- const CloudMesh::PointT& p= cloud->points[(*indices)[i]];
- data->SetValue (2*j*3 + 0, p.x);
- data->SetValue (2*j*3 + 1, p.y);
- data->SetValue (2*j*3 + 2, p.z);
- data->SetValue (2*j*3 + 3, float (p.x + p.normal_x*scale_));
- data->SetValue (2*j*3 + 4, float (p.y + p.normal_y*scale_));
- data->SetValue (2*j*3 + 5, float (p.z + p.normal_z*scale_));
+ vtkIdType nr_normals = static_cast<vtkIdType>((indices->size() - 1) / level_ + 1);
+ data->SetNumberOfValues(2 * 3 * nr_normals);
+ for (vtkIdType i = 0, j = 0; j < nr_normals;
+ j++, i = static_cast<vtkIdType>(j * level_)) {
+ const CloudMesh::PointT& p = cloud->points[(*indices)[i]];
+ data->SetValue(2 * j * 3 + 0, p.x);
+ data->SetValue(2 * j * 3 + 1, p.y);
+ data->SetValue(2 * j * 3 + 2, p.z);
+ data->SetValue(2 * j * 3 + 3, float(p.x + p.normal_x * scale_));
+ data->SetValue(2 * j * 3 + 4, float(p.y + p.normal_y * scale_));
+ data->SetValue(2 * j * 3 + 5, float(p.z + p.normal_z * scale_));
lines->InsertNextCell(2);
- lines->InsertCellPoint(2*j);
- lines->InsertCellPoint(2*j + 1);
+ lines->InsertCellPoint(2 * j);
+ lines->InsertCellPoint(2 * j + 1);
}
}
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::NormalsActorItem::initImpl()
{
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
- points->SetDataTypeToFloat ();
+ points->SetDataTypeToFloat();
vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
poly_data_->SetPoints(points);
createNormalLines();
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
- mapper->SetInputData (poly_data_);
+ mapper->SetInputData(poly_data_);
vtkSmartPointer<vtkDataArray> scalars;
cloud_mesh_->getColorScalarsFromField(scalars, color_scheme_);
- poly_data_->GetPointData ()->SetScalars (scalars);
+ poly_data_->GetPointData()->SetScalars(scalars);
double minmax[2];
scalars->GetRange(minmax);
mapper->SetScalarRange(minmax);
mapper->SetColorModeToMapScalars();
mapper->SetScalarModeToUsePointData();
- vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>(dynamic_cast<vtkLODActor*>(actor_.GetPointer()));
+ vtkSmartPointer<vtkLODActor> actor =
+ vtkSmartPointer<vtkLODActor>(dynamic_cast<vtkLODActor*>(actor_.GetPointer()));
actor->SetMapper(mapper);
}
vtkSmartPointer<vtkDataArray> scalars;
cloud_mesh_->getColorScalarsFromField(scalars, color_scheme_);
- poly_data_->GetPointData ()->SetScalars (scalars);
+ poly_data_->GetPointData()->SetScalars(scalars);
double minmax[2];
scalars->GetRange(minmax);
actor_->GetMapper()->SetScalarRange(minmax);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::NormalsActorItem::prepareContextMenu(QMenu* ) const
-{
-
-}
+pcl::modeler::NormalsActorItem::prepareContextMenu(QMenu*) const
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::NormalsActorItem::prepareProperties(ParameterDialog* )
-{
-
-}
+pcl::modeler::NormalsActorItem::prepareProperties(ParameterDialog*)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::NormalsActorItem::setProperties()
-{
-
-}
+{}
#include <pcl/apps/modeler/parameter.h>
-#include <cassert>
-#include <fstream>
-#include <iomanip>
-
#include <QAbstractItemModel>
#include <QCheckBox>
#include <QColorDialog>
#include <QSpinBox>
+#include <cassert>
+#include <fstream>
+#include <iomanip>
+
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::Parameter::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index)
+pcl::modeler::Parameter::setModelData(QWidget* editor,
+ QAbstractItemModel* model,
+ const QModelIndex& index)
{
getEditorData(editor);
std::pair<QVariant, int> model_data = toModelData();
model->setData(index, model_data.first, model_data.second);
}
-
//////////////////////////////////////////////////////////////////////////////////////////////
std::string
-pcl::modeler::IntParameter::valueTip()
+pcl::modeler::IntParameter::valueTip()
{
return QString("value range: [%1, %3]").arg(low_).arg(high_).toStdString();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-QWidget *
-pcl::modeler::IntParameter::createEditor(QWidget *parent)
+QWidget*
+pcl::modeler::IntParameter::createEditor(QWidget* parent)
{
- QSpinBox *editor = new QSpinBox(parent);
+ QSpinBox* editor = new QSpinBox(parent);
editor->setMinimum(low_);
editor->setMaximum(high_);
editor->setSingleStep(step_);
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::IntParameter::setEditorData(QWidget *editor)
+pcl::modeler::IntParameter::setEditorData(QWidget* editor)
{
- QSpinBox *spinBox = static_cast<QSpinBox*>(editor);
+ QSpinBox* spinBox = static_cast<QSpinBox*>(editor);
spinBox->setAlignment(Qt::AlignHCenter);
- int value = int (*this);
+ int value = int(*this);
spinBox->setValue(value);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::IntParameter::getEditorData(QWidget *editor)
+pcl::modeler::IntParameter::getEditorData(QWidget* editor)
{
- QSpinBox *spinBox = static_cast<QSpinBox*>(editor);
+ QSpinBox* spinBox = static_cast<QSpinBox*>(editor);
int value = spinBox->text().toInt();
current_value_ = value;
}
pcl::modeler::IntParameter::toModelData()
{
std::pair<QVariant, int> model_data;
- model_data.first = int (*this);
+ model_data.first = int(*this);
model_data.second = Qt::EditRole;
- return (model_data);
+ return model_data;
}
//////////////////////////////////////////////////////////////////////////////////////////////
std::string
-pcl::modeler::BoolParameter::valueTip()
+pcl::modeler::BoolParameter::valueTip()
{
return QString("bool value").toStdString();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-QWidget *
-pcl::modeler::BoolParameter::createEditor(QWidget *parent)
+QWidget*
+pcl::modeler::BoolParameter::createEditor(QWidget* parent)
{
- QCheckBox *editor = new QCheckBox(parent);
+ QCheckBox* editor = new QCheckBox(parent);
return editor;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::BoolParameter::setEditorData(QWidget *editor)
+pcl::modeler::BoolParameter::setEditorData(QWidget* editor)
{
- QCheckBox *checkBox = static_cast<QCheckBox*>(editor);
+ QCheckBox* checkBox = static_cast<QCheckBox*>(editor);
- bool value = bool (*this);
- checkBox->setCheckState(value?(Qt::Checked):(Qt::Unchecked));
+ bool value = bool(*this);
+ checkBox->setCheckState(value ? (Qt::Checked) : (Qt::Unchecked));
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
- pcl::modeler::BoolParameter::getEditorData(QWidget *editor)
+pcl::modeler::BoolParameter::getEditorData(QWidget* editor)
{
- QCheckBox *checkBox = static_cast<QCheckBox*>(editor);
+ QCheckBox* checkBox = static_cast<QCheckBox*>(editor);
bool value = (checkBox->checkState() == Qt::Checked);
current_value_ = value;
}
pcl::modeler::BoolParameter::toModelData()
{
std::pair<QVariant, int> model_data;
- model_data.first = bool (*this);
+ model_data.first = bool(*this);
model_data.second = Qt::EditRole;
- return (model_data);
+ return model_data;
}
//////////////////////////////////////////////////////////////////////////////////////////////
std::string
-pcl::modeler::DoubleParameter::valueTip()
+pcl::modeler::DoubleParameter::valueTip()
{
return QString("value range: [%1, %3]").arg(low_).arg(high_).toStdString();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-QWidget *
-pcl::modeler::DoubleParameter::createEditor(QWidget *parent)
+QWidget*
+pcl::modeler::DoubleParameter::createEditor(QWidget* parent)
{
- QDoubleSpinBox *editor = new QDoubleSpinBox(parent);
+ QDoubleSpinBox* editor = new QDoubleSpinBox(parent);
editor->setMinimum(low_);
editor->setMaximum(high_);
editor->setSingleStep(step_);
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::DoubleParameter::setEditorData(QWidget *editor)
+pcl::modeler::DoubleParameter::setEditorData(QWidget* editor)
{
- QDoubleSpinBox *spinBox = static_cast<QDoubleSpinBox*>(editor);
+ QDoubleSpinBox* spinBox = static_cast<QDoubleSpinBox*>(editor);
spinBox->setAlignment(Qt::AlignHCenter);
- double value = double (*this);
+ double value = double(*this);
spinBox->setValue(value);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::DoubleParameter::getEditorData(QWidget *editor)
+pcl::modeler::DoubleParameter::getEditorData(QWidget* editor)
{
- QDoubleSpinBox *spinBox = static_cast<QDoubleSpinBox*>(editor);
+ QDoubleSpinBox* spinBox = static_cast<QDoubleSpinBox*>(editor);
double value = spinBox->text().toDouble();
current_value_ = value;
}
pcl::modeler::DoubleParameter::toModelData()
{
std::pair<QVariant, int> model_data;
- model_data.first = double (*this);
+ model_data.first = double(*this);
model_data.second = Qt::EditRole;
- return (model_data);
+ return model_data;
}
//////////////////////////////////////////////////////////////////////////////////////////////
std::string
-pcl::modeler::ColorParameter::valueTip()
+pcl::modeler::ColorParameter::valueTip()
{
- return ("Color");
+ return "Color";
}
//////////////////////////////////////////////////////////////////////////////////////////////
-QWidget *
-pcl::modeler::ColorParameter::createEditor(QWidget *parent)
+QWidget*
+pcl::modeler::ColorParameter::createEditor(QWidget* parent)
{
- QColorDialog *editor = new QColorDialog(parent);
+ QColorDialog* editor = new QColorDialog(parent);
return editor;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::ColorParameter::setEditorData(QWidget *editor)
+pcl::modeler::ColorParameter::setEditorData(QWidget* editor)
{
- QColorDialog *color_dialog = static_cast<QColorDialog*>(editor);
+ QColorDialog* color_dialog = static_cast<QColorDialog*>(editor);
- QColor value = QColor (*this);
+ QColor value = QColor(*this);
color_dialog->setCurrentColor(value);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::ColorParameter::getEditorData(QWidget *editor)
+pcl::modeler::ColorParameter::getEditorData(QWidget* editor)
{
- QColorDialog *color_dialog = static_cast<QColorDialog*>(editor);
+ QColorDialog* color_dialog = static_cast<QColorDialog*>(editor);
QColor value = color_dialog->currentColor();
current_value_ = value;
std::pair<QVariant, int> model_data;
model_data.first = QBrush(QColor(*this));
model_data.second = Qt::BackgroundRole;
- return (model_data);
+ return model_data;
}
/*
-* Software License Agreement (BSD License)
-*
-* Point Cloud Library (PCL) - www.pointclouds.org
-* Copyright (c) 2012, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-*/
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
-#include <pcl/apps/modeler/parameter_dialog.h>
#include <pcl/apps/modeler/parameter.h>
-
-#include <cassert>
-#include <fstream>
+#include <pcl/apps/modeler/parameter_dialog.h>
#include <QCheckBox>
#include <QColorDialog>
#include <QPushButton>
#include <QTableView>
+#include <cassert>
+#include <fstream>
+
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::ParameterDialog::addParameter(pcl::modeler::Parameter* parameter)
{
- if (name_parameter_map_.find(parameter->getName()) == name_parameter_map_.end())
- {
+ if (name_parameter_map_.find(parameter->getName()) == name_parameter_map_.end()) {
name_parameter_map_.insert(std::make_pair(parameter->getName(), parameter));
}
- else
- {
+ else {
assert(false);
}
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::ParameterDialog::ParameterDialog(const std::string& title, QWidget* parent) : QDialog(parent), parameter_model_(nullptr)
+pcl::modeler::ParameterDialog::ParameterDialog(const std::string& title,
+ QWidget* parent)
+: QDialog(parent), parameter_model_(nullptr)
{
setModal(false);
- setWindowTitle(QString(title.c_str())+" Parameters");
+ setWindowTitle(QString(title.c_str()) + " Parameters");
}
//////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::modeler::ParameterDialog::exec()
{
- pcl::modeler::ParameterModel parameterModel (int (name_parameter_map_.size ()), 2, this);
+ pcl::modeler::ParameterModel parameterModel(int(name_parameter_map_.size()), 2, this);
parameter_model_ = ¶meterModel;
QStringList headerLabels;
tableView.setModel(¶meterModel);
std::size_t currentRow = 0;
- for(const auto &name_parameter : name_parameter_map_)
- {
- QModelIndex name = parameterModel.index(int (currentRow), 0, QModelIndex());
+ for (const auto& name_parameter : name_parameter_map_) {
+ QModelIndex name = parameterModel.index(int(currentRow), 0, QModelIndex());
parameterModel.setData(name, QVariant(name_parameter.first.c_str()));
- QModelIndex value = parameterModel.index(int (currentRow), 1, QModelIndex());
+ QModelIndex value = parameterModel.index(int(currentRow), 1, QModelIndex());
std::pair<QVariant, int> model_data = name_parameter.second->toModelData();
parameterModel.setData(value, model_data.first, model_data.second);
- currentRow ++;
+ currentRow++;
}
ParameterDelegate parameterDelegate(name_parameter_map_);
tableView.setSelectionBehavior(QAbstractItemView::SelectRows);
tableView.resizeColumnsToContents();
- int totlen = tableView.columnWidth(0) + tableView.columnWidth(1) + frameSize().width();
+ int totlen =
+ tableView.columnWidth(0) + tableView.columnWidth(1) + frameSize().width();
setMinimumWidth(totlen);
QPushButton* pushButtonReset = new QPushButton("Reset", this);
pcl::modeler::ParameterDialog::reset()
{
std::size_t currentRow = 0;
- for (auto &name_parameter : name_parameter_map_)
- {
+ for (auto& name_parameter : name_parameter_map_) {
name_parameter.second->reset();
- QModelIndex value = parameter_model_->index(int (currentRow), 1, QModelIndex());
+ QModelIndex value = parameter_model_->index(int(currentRow), 1, QModelIndex());
std::pair<QVariant, int> model_data = name_parameter.second->toModelData();
parameter_model_->setData(value, model_data.first, model_data.second);
- currentRow ++;
+ currentRow++;
}
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::Parameter* pcl::modeler::ParameterDelegate::getCurrentParameter(const QModelIndex &index) const
+pcl::modeler::Parameter*
+pcl::modeler::ParameterDelegate::getCurrentParameter(const QModelIndex& index) const
{
std::map<std::string, Parameter*>::iterator currentParameter = parameter_map_.begin();
std::size_t currentRow = 0;
- while(currentRow < (std::size_t) index.row() && currentParameter != parameter_map_.end()) {
- ++ currentParameter;
- ++ currentRow;
+ while (currentRow < (std::size_t)index.row() &&
+ currentParameter != parameter_map_.end()) {
+ ++currentParameter;
+ ++currentRow;
}
assert(currentParameter != parameter_map_.end());
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::ParameterDelegate::ParameterDelegate(std::map<std::string, Parameter*>& parameterMap, QObject *parent):
- QStyledItemDelegate(parent),
- parameter_map_(parameterMap)
-{
-}
+pcl::modeler::ParameterDelegate::ParameterDelegate(
+ std::map<std::string, Parameter*>& parameterMap, QObject* parent)
+: QStyledItemDelegate(parent), parameter_map_(parameterMap)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-QWidget *
-pcl::modeler::ParameterDelegate::createEditor(QWidget *parent, const QStyleOptionViewItem &, const QModelIndex &index) const
+QWidget*
+pcl::modeler::ParameterDelegate::createEditor(QWidget* parent,
+ const QStyleOptionViewItem&,
+ const QModelIndex& index) const
{
return getCurrentParameter(index)->createEditor(parent);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::ParameterDelegate::setEditorData(QWidget *editor, const QModelIndex &index) const
+pcl::modeler::ParameterDelegate::setEditorData(QWidget* editor,
+ const QModelIndex& index) const
{
getCurrentParameter(index)->setEditorData(editor);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::ParameterDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const
+pcl::modeler::ParameterDelegate::setModelData(QWidget* editor,
+ QAbstractItemModel* model,
+ const QModelIndex& index) const
{
getCurrentParameter(index)->setModelData(editor, model, index);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::ParameterDelegate::updateEditorGeometry(QWidget *editor, const QStyleOptionViewItem &option, const QModelIndex &) const
+pcl::modeler::ParameterDelegate::updateEditorGeometry(
+ QWidget* editor, const QStyleOptionViewItem& option, const QModelIndex&) const
{
editor->setGeometry(option.rect);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::ParameterDelegate::initStyleOption(QStyleOptionViewItem *option, const QModelIndex &index) const
+pcl::modeler::ParameterDelegate::initStyleOption(QStyleOptionViewItem* option,
+ const QModelIndex& index) const
{
option->displayAlignment |= Qt::AlignHCenter;
QStyledItemDelegate::initStyleOption(option, index);
*
*/
-#include <pcl/apps/modeler/points_actor_item.h>
-
#include <pcl/apps/modeler/cloud_mesh.h>
+#include <pcl/apps/modeler/points_actor_item.h>
-#include <vtkVertexGlyphFilter.h>
-#include <vtkLODActor.h>
#include <vtkDataSetMapper.h>
+#include <vtkLODActor.h>
#include <vtkPointData.h>
#include <vtkProperty.h>
-
+#include <vtkVertexGlyphFilter.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::PointsActorItem::PointsActorItem(QTreeWidgetItem* parent,
- const CloudMesh::Ptr& cloud_mesh,
- const vtkSmartPointer<vtkRenderWindow>& render_window)
- :ChannelActorItem(parent, cloud_mesh, render_window, vtkSmartPointer<vtkLODActor>::New(), "Points")
-{
-}
+pcl::modeler::PointsActorItem::PointsActorItem(
+ QTreeWidgetItem* parent,
+ const CloudMesh::Ptr& cloud_mesh,
+ const vtkSmartPointer<vtkRenderWindow>& render_window)
+: ChannelActorItem(
+ parent, cloud_mesh, render_window, vtkSmartPointer<vtkLODActor>::New(), "Points")
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::PointsActorItem::~PointsActorItem ()
-{
-
-}
+pcl::modeler::PointsActorItem::~PointsActorItem() {}
//////////////////////////////////////////////////////////////////////////////////////////////
void
{
poly_data_->SetPoints(cloud_mesh_->getVtkPoints());
- vtkSmartPointer<vtkVertexGlyphFilter> vertex_glyph_filter = vtkSmartPointer<vtkVertexGlyphFilter>::New();
- vertex_glyph_filter->AddInputData (poly_data_);
+ vtkSmartPointer<vtkVertexGlyphFilter> vertex_glyph_filter =
+ vtkSmartPointer<vtkVertexGlyphFilter>::New();
+ vertex_glyph_filter->AddInputData(poly_data_);
vertex_glyph_filter->Update();
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
vtkSmartPointer<vtkDataArray> scalars;
cloud_mesh_->getColorScalarsFromField(scalars, color_scheme_);
- poly_data_->GetPointData ()->SetScalars (scalars);
+ poly_data_->GetPointData()->SetScalars(scalars);
double minmax[2];
scalars->GetRange(minmax);
mapper->ImmediateModeRenderingOff();
#endif
- vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>(dynamic_cast<vtkLODActor*>(actor_.GetPointer()));
+ vtkSmartPointer<vtkLODActor> actor =
+ vtkSmartPointer<vtkLODActor>(dynamic_cast<vtkLODActor*>(actor_.GetPointer()));
actor->SetMapper(mapper);
- actor->SetNumberOfCloudPoints(int(std::max<vtkIdType> (1, poly_data_->GetNumberOfPoints () / 10)));
+ actor->SetNumberOfCloudPoints(
+ int(std::max<vtkIdType>(1, poly_data_->GetNumberOfPoints() / 10)));
actor->GetProperty()->SetInterpolationToFlat();
}
{
vtkSmartPointer<vtkDataArray> scalars;
cloud_mesh_->getColorScalarsFromField(scalars, color_scheme_);
- poly_data_->GetPointData ()->SetScalars (scalars);
+ poly_data_->GetPointData()->SetScalars(scalars);
double minmax[2];
scalars->GetRange(minmax);
actor_->GetMapper()->SetScalarRange(minmax);
-
- return;
}
-
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::PointsActorItem::prepareContextMenu(QMenu*) const
-{
-
-}
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::PointsActorItem::prepareProperties(ParameterDialog*)
-{
-
-}
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::PointsActorItem::setProperties()
-{
-
-}
+{}
*
*/
-#include <pcl/apps/modeler/poisson_worker.h>
-#include <pcl/apps/modeler/parameter_dialog.h>
-#include <pcl/apps/modeler/parameter.h>
#include <pcl/apps/modeler/cloud_mesh.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
-#include <pcl/surface/poisson.h>
+#include <pcl/apps/modeler/parameter.h>
+#include <pcl/apps/modeler/parameter_dialog.h>
+#include <pcl/apps/modeler/poisson_worker.h>
#include <pcl/surface/impl/poisson.hpp>
+#include <pcl/surface/poisson.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::PoissonReconstructionWorker::PoissonReconstructionWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent) :
- AbstractWorker(cloud_mesh_items, parent),
- depth_(nullptr), solver_divide_(nullptr), iso_divide_(nullptr), degree_(nullptr), scale_(nullptr), samples_per_node_(nullptr)
-{
-
-}
+pcl::modeler::PoissonReconstructionWorker::PoissonReconstructionWorker(
+ const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent)
+: AbstractWorker(cloud_mesh_items, parent)
+, depth_(nullptr)
+, solver_divide_(nullptr)
+, iso_divide_(nullptr)
+, degree_(nullptr)
+, scale_(nullptr)
+, samples_per_node_(nullptr)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::PoissonReconstructionWorker::~PoissonReconstructionWorker()
pcl::modeler::PoissonReconstructionWorker::setupParameters()
{
pcl::Poisson<pcl::PointSurfel> poisson;
- depth_ = new IntParameter("Maximum Tree Depth",
- "Maximum depth of the tree that will be used for surface reconstruction. \
- Running at depth d corresponds to solving on a voxel grid whose resolution \
- is no larger than 2^d x 2^d x 2^d. Note that since the reconstructor adapts \
- the octree to the sampling density, the specified reconstruction depth \
- is only an upper bound.",
- poisson.getDepth(), 2, 16);
-
- solver_divide_ = new IntParameter("Solver Divide",
- "The depth at which a block Gauss-Seidel solver is used to solve the Laplacian \
- equation. Using this parameter helps reduce the memory overhead at the cost of \
- a small increase in reconstruction time. (In practice, we have found that for \
- reconstructions of depth 9 or higher a subdivide depth of 7 or 8 can greatly \
- reduce the memory usage.)",
- poisson.getSolverDivide(), 2, 16);
- iso_divide_ = new IntParameter("Iso Divide",
- "Depth at which a block iso-surface extractor should be used to extract the \
- iso-surface. Using this parameter helps reduce the memory overhead at the cost \
- of a small increase in extraction time. (In practice, we have found that for \
- reconstructions of depth 9 or higher a subdivide depth of 7 or 8 can greatly \
- reduce the memory usage.)",
- poisson.getIsoDivide(), 2, 16);
+ // clang-format off
+ depth_ = new IntParameter(
+ "Maximum Tree Depth",
+ "Maximum depth of the tree that will be used for surface reconstruction. "
+ "Running at depth d corresponds to solving on a voxel grid whose resolution "
+ "is no larger than 2^d x 2^d x 2^d. Note that since the reconstructor adapts "
+ "the octree to the sampling density, the specified reconstruction depth "
+ "is only an upper bound.",
+ poisson.getDepth(), 2, 16);
+
+ solver_divide_ = new IntParameter(
+ "Solver Divide",
+ "The depth at which a block Gauss-Seidel solver is used to solve the Laplacian "
+ "equation. Using this parameter helps reduce the memory overhead at the cost of "
+ "a small increase in reconstruction time. (In practice, we have found that for "
+ "reconstructions of depth 9 or higher a subdivide depth of 7 or 8 can greatly "
+ "reduce the memory usage.)",
+ poisson.getSolverDivide(), 2, 16);
+
+ iso_divide_ = new IntParameter(
+ "Iso Divide",
+ "Depth at which a block iso-surface extractor should be used to extract the "
+ "iso-surface. Using this parameter helps reduce the memory overhead at the cost "
+ "of a small increase in extraction time. (In practice, we have found that for "
+ "reconstructions of depth 9 or higher a subdivide depth of 7 or 8 can greatly "
+ "reduce the memory usage.)",
+ poisson.getIsoDivide(), 2, 16);
degree_ = new IntParameter("Degree", "Degree", poisson.getDegree(), 1, 5);
- scale_ = new DoubleParameter("Scale",
- "The ratio between the diameter of the cube used for reconstruction and the \
- diameter of the samples' bounding cube.",
- poisson.getScale(), 0.1, 10.0, 0.01);
-
- samples_per_node_ = new DoubleParameter("Samples Per Node",
- "The minimum number of sample points that should fall within an octree node as \
- the octree construction is adapted to sampling density. For noise-free samples, small \
- values in the range [1.0 - 5.0] can be used. For more noisy samples, larger values in \
- the range [15.0 - 20.0] may be needed to provide a smoother, noise-reduced, reconstruction.",
- poisson.getScale(), 0.1, 10.0, 0.01);
+ scale_ = new DoubleParameter(
+ "Scale",
+ "The ratio between the diameter of the cube used for reconstruction and the "
+ "diameter of the samples' bounding cube.",
+ poisson.getScale(), 0.1, 10.0, 0.01);
+
+ samples_per_node_ = new DoubleParameter(
+ "Samples Per Node",
+ "The minimum number of sample points that should fall within an octree node as "
+ "the octree construction is adapted to sampling density. For noise-free samples, "
+ "small values in the range [1.0 - 5.0] can be used. For more noisy samples, "
+ "larger values in the range [15.0 - 20.0] may be needed to provide a smoother, "
+ "noise-reduced, reconstruction.",
+ poisson.getScale(), 0.1, 10.0, 0.01);
+ // clang-format on
parameter_dialog_->addParameter(depth_);
parameter_dialog_->addParameter(solver_divide_);
parameter_dialog_->addParameter(degree_);
parameter_dialog_->addParameter(scale_);
parameter_dialog_->addParameter(samples_per_node_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
poisson.setSolverDivide(*solver_divide_);
poisson.setIsoDivide(*iso_divide_);
poisson.setDegree(*degree_);
- poisson.setScale (float (*scale_));
- poisson.setScale (float (*samples_per_node_));
+ poisson.setScale(float(*scale_));
+ poisson.setScale(float(*samples_per_node_));
poisson.setConfidence(true);
poisson.setManifold(true);
CloudMesh::PointCloudPtr cloud(new CloudMesh::PointCloud());
poisson.reconstruct(*cloud, cloud_mesh_item->getCloudMesh()->getPolygons());
cloud_mesh_item->getCloudMesh()->getCloud() = cloud;
-
- return;
}
*
*/
+#include <pcl/apps/modeler/dock_widget.h>
+#include <pcl/apps/modeler/main_window.h>
#include <pcl/apps/modeler/render_window.h>
#include <pcl/apps/modeler/render_window_item.h>
#include <pcl/apps/modeler/scene_tree.h>
-#include <pcl/apps/modeler/dock_widget.h>
-#include <pcl/apps/modeler/main_window.h>
-#include <vtkProp.h>
-#include <vtkRenderer.h>
+
#include <vtkBoundingBox.h>
-#include <vtkSmartPointer.h>
-#include <vtkRenderWindow.h>
#include <vtkCubeAxesActor.h>
+#include <vtkProp.h>
+#include <vtkRenderWindow.h>
+#include <vtkRenderer.h>
#include <vtkRendererCollection.h>
-
+#include <vtkSmartPointer.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::RenderWindow::RenderWindow(RenderWindowItem* render_window_item, QWidget *parent, Qt::WindowFlags flags)
- : QVTKWidget(parent, flags),
- axes_(vtkSmartPointer<vtkCubeAxesActor>::New()),
- render_window_item_(render_window_item)
+pcl::modeler::RenderWindow::RenderWindow(RenderWindowItem* render_window_item,
+ QWidget* parent,
+ Qt::WindowFlags flags)
+: QVTKWidget(parent, flags)
+, axes_(vtkSmartPointer<vtkCubeAxesActor>::New())
+, render_window_item_(render_window_item)
{
setFocusPolicy(Qt::StrongFocus);
initRenderer();
pcl::modeler::RenderWindow::~RenderWindow()
{
DockWidget* dock_widget = dynamic_cast<DockWidget*>(parent());
- if (dock_widget != nullptr)
- {
+ if (dock_widget != nullptr) {
MainWindow::getInstance().removeDockWidget(dock_widget);
dock_widget->deleteLater();
}
-
- return;
}
/////////////////////////////////////////////////////////////////////////////////////////////
win->AddRenderer(renderer);
// FPS callback
- //vtkSmartPointer<vtkTextActor> txt = vtkSmartPointer<vtkTextActor>::New ();
- //using FPSCallback = pcl::visualization::FPSCallback;
- //vtkSmartPointer<FPSCallback> update_fps = vtkSmartPointer<FPSCallback>::New ();
- //update_fps->setTextActor (txt);
- //renderer->AddObserver (vtkCommand::EndEvent, update_fps);
- //renderer->AddActor (txt);
+ // vtkSmartPointer<vtkTextActor> txt = vtkSmartPointer<vtkTextActor>::New ();
+ // using FPSCallback = pcl::visualization::FPSCallback;
+ // vtkSmartPointer<FPSCallback> update_fps = vtkSmartPointer<FPSCallback>::New ();
+ // update_fps->setTextActor (txt);
+ // renderer->AddObserver (vtkCommand::EndEvent, update_fps);
+ // renderer->AddActor (txt);
// Set up render window
- win->AlphaBitPlanesOff ();
- win->PointSmoothingOff ();
- win->LineSmoothingOff ();
- win->PolygonSmoothingOff ();
- win->SwapBuffersOn ();
- win->SetStereoTypeToAnaglyph ();
- win->GetInteractor()->SetDesiredUpdateRate (30.0);
-
- return;
+ win->AlphaBitPlanesOff();
+ win->PointSmoothingOff();
+ win->LineSmoothingOff();
+ win->PolygonSmoothingOff();
+ win->SwapBuffersOn();
+ win->SetStereoTypeToAnaglyph();
+ win->GetInteractor()->SetDesiredUpdateRate(30.0);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::RenderWindow::focusInEvent(QFocusEvent * event)
+pcl::modeler::RenderWindow::focusInEvent(QFocusEvent* event)
{
- dynamic_cast<SceneTree*>(render_window_item_->treeWidget())->selectRenderWindowItem(render_window_item_);
+ dynamic_cast<SceneTree*>(render_window_item_->treeWidget())
+ ->selectRenderWindowItem(render_window_item_);
QVTKWidget::focusInEvent(event);
}
DockWidget* dock_widget = dynamic_cast<DockWidget*>(parent());
if (dock_widget != nullptr)
dock_widget->setFocusBasedStyle(flag);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
DockWidget* dock_widget = dynamic_cast<DockWidget*>(parent());
if (dock_widget != nullptr)
dock_widget->setWindowTitle(title);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::RenderWindow::resetCamera()
{
double bounds[6];
- GetRenderWindow()->GetRenderers()->GetFirstRenderer()->ComputeVisiblePropBounds(bounds);
+ GetRenderWindow()->GetRenderers()->GetFirstRenderer()->ComputeVisiblePropBounds(
+ bounds);
GetRenderWindow()->GetRenderers()->GetFirstRenderer()->ResetCamera(bounds);
render();
}
GetRenderWindow()->GetRenderers()->GetFirstRenderer()->SetBackground(r, g, b);
}
-
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::RenderWindow::updateAxes()
{
vtkBoundingBox bb;
- vtkActorCollection* actors = GetRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActors();
+ vtkActorCollection* actors =
+ GetRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActors();
actors->InitTraversal();
- for (int i = 0, i_end = actors->GetNumberOfItems(); i < i_end; ++ i)
- {
+ for (int i = 0, i_end = actors->GetNumberOfItems(); i < i_end; ++i) {
vtkActor* actor = actors->GetNextActor();
if (actor == axes_.GetPointer())
continue;
double bounds[6];
bb.GetBounds(bounds);
axes_->SetBounds(bounds);
- axes_->SetCamera(GetRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera());
+ axes_->SetCamera(
+ GetRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera());
}
//////////////////////////////////////////////////////////////////////////////////////////////
GetRenderWindow()->GetRenderers()->GetFirstRenderer()->AddActor(axes_);
else
GetRenderWindow()->GetRenderers()->GetFirstRenderer()->RemoveActor(axes_);
-
- return;
}
*
*/
-#include <pcl/apps/modeler/render_window_item.h>
-#include <pcl/apps/modeler/render_window.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
#include <pcl/apps/modeler/main_window.h>
#include <pcl/apps/modeler/parameter.h>
#include <pcl/apps/modeler/parameter_dialog.h>
-
+#include <pcl/apps/modeler/render_window.h>
+#include <pcl/apps/modeler/render_window_item.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::RenderWindowItem::RenderWindowItem(QTreeWidget * parent)
- : QTreeWidgetItem(parent),
- render_window_(new RenderWindow(this)),
- background_color_(new ColorParameter("Background Color", "The background color of the render window", QColor(0, 0, 0))),
- show_axes_(new BoolParameter("Show Axes", "Show Axes", true))
+pcl::modeler::RenderWindowItem::RenderWindowItem(QTreeWidget* parent)
+: QTreeWidgetItem(parent)
+, render_window_(new RenderWindow(this))
+, background_color_(new ColorParameter(
+ "Background Color", "The background color of the render window", QColor(0, 0, 0)))
+, show_axes_(new BoolParameter("Show Axes", "Show Axes", true))
{
- setFlags(flags()&(~Qt::ItemIsDragEnabled));
+ setFlags(flags() & (~Qt::ItemIsDragEnabled));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::RenderWindowItem::~RenderWindowItem()
-{
- render_window_->deleteLater();
-}
+pcl::modeler::RenderWindowItem::~RenderWindowItem() { render_window_->deleteLater(); }
//////////////////////////////////////////////////////////////////////////////////////////////
bool
CloudMeshItem* cloud_mesh_item = new CloudMeshItem(this, filename.toStdString());
addChild(cloud_mesh_item);
- if (!cloud_mesh_item->open())
- {
+ if (!cloud_mesh_item->open()) {
removeChild(cloud_mesh_item);
delete cloud_mesh_item;
- return (false);
+ return false;
}
treeWidget()->setCurrentItem(cloud_mesh_item);
- return (true);
+ return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
treeWidget()->setCurrentItem(cloud_mesh_item);
- return (cloud_mesh_item);
+ return cloud_mesh_item;
}
//////////////////////////////////////////////////////////////////////////////////////////////
{
double r, g, b;
render_window_->getBackground(r, g, b);
- QColor color(static_cast<int> (r*255), static_cast<int> (g*255), static_cast<int> (b*255));
+ QColor color(
+ static_cast<int>(r * 255), static_cast<int>(g * 255), static_cast<int>(b * 255));
background_color_->setDefaultValue(color);
parameter_dialog->addParameter(background_color_);
parameter_dialog->addParameter(show_axes_);
pcl::modeler::RenderWindowItem::setProperties()
{
QColor color = *background_color_;
- render_window_->setBackground(color.red()/255.0, color.green()/255.0, color.blue()/255.0);
+ render_window_->setBackground(
+ color.red() / 255.0, color.green() / 255.0, color.blue() / 255.0);
render_window_->setShowAxes(*show_axes_);
-
- return;
}
*
*/
-#include <pcl/apps/modeler/scene_tree.h>
-
-#include <set>
-
-#include <QContextMenuEvent>
-#include <QFileDialog>
-#include <QMessageBox>
-
+#include <pcl/apps/modeler/cloud_mesh_item.h>
+#include <pcl/apps/modeler/cloud_mesh_item_updater.h>
+#include <pcl/apps/modeler/icp_registration_worker.h>
#include <pcl/apps/modeler/main_window.h>
+#include <pcl/apps/modeler/normal_estimation_worker.h>
+#include <pcl/apps/modeler/poisson_worker.h>
#include <pcl/apps/modeler/render_window.h>
#include <pcl/apps/modeler/render_window_item.h>
-#include <pcl/apps/modeler/cloud_mesh_item.h>
-#include <pcl/apps/modeler/cloud_mesh_item_updater.h>
+#include <pcl/apps/modeler/scene_tree.h>
+#include <pcl/apps/modeler/statistical_outlier_removal_worker.h>
#include <pcl/apps/modeler/thread_controller.h>
#include <pcl/apps/modeler/voxel_grid_downsample_worker.h>
-#include <pcl/apps/modeler/statistical_outlier_removal_worker.h>
-#include <pcl/apps/modeler/normal_estimation_worker.h>
-#include <pcl/apps/modeler/icp_registration_worker.h>
-#include <pcl/apps/modeler/poisson_worker.h>
#include <pcl/io/pcd_io.h>
+#include <QContextMenuEvent>
+#include <QFileDialog>
+#include <QMessageBox>
+
+#include <set>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::SceneTree::SceneTree(QWidget * parent)
- : QTreeWidget(parent)
+pcl::modeler::SceneTree::SceneTree(QWidget* parent) : QTreeWidget(parent)
{
setDragEnabled(true);
setAcceptDrops(true);
setSelectionMode(QAbstractItemView::ExtendedSelection);
- connect(selectionModel(), SIGNAL(selectionChanged(QItemSelection, QItemSelection)),
- this, SLOT(slotUpdateOnSelectionChange(QItemSelection, const QItemSelection)));
+ connect(selectionModel(),
+ SIGNAL(selectionChanged(QItemSelection, QItemSelection)),
+ this,
+ SLOT(slotUpdateOnSelectionChange(QItemSelection, const QItemSelection)));
- connect(this, SIGNAL(itemInsertedOrRemoved()),this, SLOT(slotUpdateOnInsertOrRemove()));
+ connect(
+ this, SIGNAL(itemInsertedOrRemoved()), this, SLOT(slotUpdateOnInsertOrRemove()));
- connect(this, SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)),this, SLOT(slotOnItemDoubleClicked(QTreeWidgetItem*)));
+ connect(this,
+ SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)),
+ this,
+ SLOT(slotOnItemDoubleClicked(QTreeWidgetItem*)));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::SceneTree::~SceneTree()
-{
-}
+pcl::modeler::SceneTree::~SceneTree() {}
//////////////////////////////////////////////////////////////////////////////////////////////
QSize
{
QList<RenderWindowItem*> selected_render_window_items;
if (topLevelItemCount() == 1)
- selected_render_window_items.push_back(dynamic_cast<RenderWindowItem*>(topLevelItem(0)));
+ selected_render_window_items.push_back(
+ dynamic_cast<RenderWindowItem*>(topLevelItem(0)));
else
selected_render_window_items = selectedTypeItems<RenderWindowItem>();
- return (selected_render_window_items);
+ return selected_render_window_items;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::SceneTree::contextMenuEvent(QContextMenuEvent *event)
+pcl::modeler::SceneTree::contextMenuEvent(QContextMenuEvent* event)
{
AbstractItem* item = dynamic_cast<AbstractItem*>(currentItem());
item->showContextMenu(&(event->globalPos()));
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::SceneTree::slotOnItemDoubleClicked(QTreeWidgetItem * item)
+pcl::modeler::SceneTree::slotOnItemDoubleClicked(QTreeWidgetItem* item)
{
AbstractItem* abstract_item = dynamic_cast<AbstractItem*>(item);
abstract_item->showPropertyEditor();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-bool
+bool
pcl::modeler::SceneTree::openPointCloud(const QString& filename)
{
QList<RenderWindowItem*> selected_render_window_items = selectedRenderWindowItems();
- for (auto &selected_render_window_item : selected_render_window_items)
- {
- if(!selected_render_window_item->openPointCloud(filename))
- return (false);
+ for (auto& selected_render_window_item : selected_render_window_items) {
+ if (!selected_render_window_item->openPointCloud(filename))
+ return false;
expandItem(selected_render_window_item);
}
emit fileOpened(filename);
- return (true);
+ return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-bool
+bool
pcl::modeler::SceneTree::savePointCloud(const QString& filename)
{
QList<CloudMeshItem*> selected_cloud_mesh_items = selectedTypeItems<CloudMeshItem>();
- return (CloudMeshItem::savePointCloud(selected_cloud_mesh_items, filename));
+ return CloudMeshItem::savePointCloud(selected_cloud_mesh_items, filename);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::SceneTree::slotOpenPointCloud()
{
MainWindow* main_window = &MainWindow::getInstance();
QList<RenderWindowItem*> selected_render_window_items = selectedRenderWindowItems();
- if (selected_render_window_items.empty())
- {
- QMessageBox::warning(main_window,
- tr("Failed to Open Point Cloud"),
- tr("Please specify in which render window(s) you want to open the point cloud(s)"));
+ if (selected_render_window_items.empty()) {
+ QMessageBox::warning(main_window,
+ tr("Failed to Open Point Cloud"),
+ tr("Please specify in which render window(s) you want to open "
+ "the point cloud(s)"));
return;
}
QStringList filenames = QFileDialog::getOpenFileNames(main_window,
- tr("Open Point Cloud"),
- main_window->getRecentFolder(),
- tr("Point Cloud(*.pcd)\n")
- );
+ tr("Open Point Cloud"),
+ main_window->getRecentFolder(),
+ tr("Point Cloud(*.pcd)\n"));
if (filenames.isEmpty())
return;
- for (const auto &render_window_item : selected_render_window_items)
- {
+ for (const auto& render_window_item : selected_render_window_items) {
QList<CloudMeshItem*> cloud_mesh_items;
- for (int i = 0, i_end = render_window_item->childCount(); i < i_end; ++ i)
- cloud_mesh_items.push_back(dynamic_cast<CloudMeshItem*>(render_window_item->child(i)));
+ for (int i = 0, i_end = render_window_item->childCount(); i < i_end; ++i)
+ cloud_mesh_items.push_back(
+ dynamic_cast<CloudMeshItem*>(render_window_item->child(i)));
closePointCloud(cloud_mesh_items);
}
for (QStringList::const_iterator filenames_it = filenames.begin();
- filenames_it != filenames.end();
- ++ filenames_it)
- {
+ filenames_it != filenames.end();
+ ++filenames_it) {
if (!openPointCloud(*filenames_it))
- QMessageBox::warning(main_window,
- tr("Failed to Open Point Cloud"),
- tr("Can not open point cloud file %1, please check if it's in valid .pcd format!").arg(*filenames_it));
+ QMessageBox::warning(main_window,
+ tr("Failed to Open Point Cloud"),
+ tr("Can not open point cloud file %1, please check if it's "
+ "in valid .pcd format!")
+ .arg(*filenames_it));
}
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::SceneTree::slotImportPointCloud()
{
MainWindow* main_window = &MainWindow::getInstance();
- if (selectedRenderWindowItems().empty())
- {
- QMessageBox::warning(main_window,
- tr("Failed to Import Point Cloud"),
- tr("Please specify in which render window(s) you want to import the point cloud(s)"));
+ if (selectedRenderWindowItems().empty()) {
+ QMessageBox::warning(main_window,
+ tr("Failed to Import Point Cloud"),
+ tr("Please specify in which render window(s) you want to "
+ "import the point cloud(s)"));
return;
}
QStringList filenames = QFileDialog::getOpenFileNames(main_window,
- tr("Import Point Cloud"),
- main_window->getRecentFolder(),
- tr("Point Cloud(*.pcd)\n")
- );
+ tr("Import Point Cloud"),
+ main_window->getRecentFolder(),
+ tr("Point Cloud(*.pcd)\n"));
if (filenames.isEmpty())
return;
for (QStringList::const_iterator filenames_it = filenames.begin();
- filenames_it != filenames.end();
- ++ filenames_it)
- {
+ filenames_it != filenames.end();
+ ++filenames_it) {
if (!openPointCloud(*filenames_it))
- QMessageBox::warning(main_window,
- tr("Failed to Import Point Cloud"),
- tr("Can not import point cloud file %1, please check if it's in valid .pcd format!").arg(*filenames_it));
+ QMessageBox::warning(
+ main_window,
+ tr("Failed to Import Point Cloud"),
+ tr("Can not import point cloud file %1 as .pcd file!").arg(*filenames_it));
}
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::SceneTree::slotSavePointCloud()
{
MainWindow* main_window = &MainWindow::getInstance();
- if (selectedTypeItems<CloudMeshItem>().empty())
- {
- QMessageBox::warning(main_window,
- tr("Failed to Save Point Cloud"),
- tr("Please specify the point cloud(s) you want to save"));
+ if (selectedTypeItems<CloudMeshItem>().empty()) {
+ QMessageBox::warning(main_window,
+ tr("Failed to Save Point Cloud"),
+ tr("Please specify the point cloud(s) you want to save"));
return;
}
QString filename = QFileDialog::getSaveFileName(main_window,
- tr("Save Point Cloud"),
- main_window->getRecentFolder(),
- tr("Save Cloud(*.pcd)\n"));
+ tr("Save Point Cloud"),
+ main_window->getRecentFolder(),
+ tr("Save Cloud(*.pcd)\n"));
if (filename.isEmpty())
return;
savePointCloud(filename);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::SceneTree::closePointCloud(const QList<CloudMeshItem*>& items)
{
QList<RenderWindowItem*> render_window_items;
- for (const auto &item : items)
- {
- RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(item->parent());
+ for (const auto& item : items) {
+ RenderWindowItem* render_window_item =
+ dynamic_cast<RenderWindowItem*>(item->parent());
if (render_window_item != nullptr)
render_window_items.push_back(render_window_item);
delete item;
}
- for (QList<RenderWindowItem*>::const_iterator render_window_items_it = render_window_items.begin();
- render_window_items_it != render_window_items.end();
- ++ render_window_items_it)
- {
+ for (QList<RenderWindowItem*>::const_iterator render_window_items_it =
+ render_window_items.begin();
+ render_window_items_it != render_window_items.end();
+ ++render_window_items_it) {
(*render_window_items_it)->getRenderWindow()->render();
}
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::modeler::SceneTree::slotClosePointCloud()
{
MainWindow* main_window = &MainWindow::getInstance();
QList<CloudMeshItem*> selected_cloud_mesh_items = selectedTypeItems<CloudMeshItem>();
- if (selected_cloud_mesh_items.empty())
- {
- QMessageBox::warning(main_window,
- tr("Failed to Close Point Cloud"),
- tr("Please specify the point cloud(s) you want to close"));
+ if (selected_cloud_mesh_items.empty()) {
+ QMessageBox::warning(main_window,
+ tr("Failed to Close Point Cloud"),
+ tr("Please specify the point cloud(s) you want to close"));
return;
}
closePointCloud(selected_cloud_mesh_items);
-
- return;
}
/////////////////////////////////////////////////////////////////////////////////////////////
QList<CloudMeshItem*> selected_cloud_mesh_items = selectedTypeItems<CloudMeshItem>();
CloudMesh::PointCloudPtr result(new CloudMesh::PointCloud());
- AbstractWorker* worker = new ICPRegistrationWorker(result, selected_cloud_mesh_items,&MainWindow::getInstance());
+ AbstractWorker* worker = new ICPRegistrationWorker(
+ result, selected_cloud_mesh_items, &MainWindow::getInstance());
ThreadController* thread_controller = new ThreadController();
QList<RenderWindowItem*> selected_render_window_items = selectedRenderWindowItems();
- for (auto &selected_render_window_item : selected_render_window_items)
- {
+ for (auto& selected_render_window_item : selected_render_window_items) {
CloudMeshItem* cloud_mesh_item = selected_render_window_item->addPointCloud(result);
expandItem(selected_render_window_item);
- connect(worker, SIGNAL(finished()), new CloudMeshItemUpdater(cloud_mesh_item), SLOT(updateCloudMeshItem()));
+ connect(worker,
+ SIGNAL(finished()),
+ new CloudMeshItemUpdater(cloud_mesh_item),
+ SLOT(updateCloudMeshItem()));
}
-
- thread_controller->runWorker(worker);
- return;
+ thread_controller->runWorker(worker);
}
/////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::SceneTree::slotVoxelGridDownsampleFilter()
{
QList<CloudMeshItem*> selected_cloud_mesh_items = selectedTypeItems<CloudMeshItem>();
- AbstractWorker* worker = new VoxelGridDownampleWorker(selected_cloud_mesh_items,&MainWindow::getInstance());
+ AbstractWorker* worker = new VoxelGridDownampleWorker(selected_cloud_mesh_items,
+ &MainWindow::getInstance());
ThreadController* thread_controller = new ThreadController();
- connect(worker, SIGNAL(dataUpdated(CloudMeshItem*)), thread_controller, SLOT(slotOnCloudMeshItemUpdate(CloudMeshItem*)));
+ connect(worker,
+ SIGNAL(dataUpdated(CloudMeshItem*)),
+ thread_controller,
+ SLOT(slotOnCloudMeshItemUpdate(CloudMeshItem*)));
thread_controller->runWorker(worker);
-
- return;
}
/////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::SceneTree::slotStatisticalOutlierRemovalFilter()
{
QList<CloudMeshItem*> selected_cloud_mesh_items = selectedTypeItems<CloudMeshItem>();
- AbstractWorker* worker = new StatisticalOutlierRemovalWorker(selected_cloud_mesh_items,&MainWindow::getInstance());
+ AbstractWorker* worker = new StatisticalOutlierRemovalWorker(
+ selected_cloud_mesh_items, &MainWindow::getInstance());
ThreadController* thread_controller = new ThreadController();
- connect(worker, SIGNAL(dataUpdated(CloudMeshItem*)), thread_controller, SLOT(slotOnCloudMeshItemUpdate(CloudMeshItem*)));
+ connect(worker,
+ SIGNAL(dataUpdated(CloudMeshItem*)),
+ thread_controller,
+ SLOT(slotOnCloudMeshItemUpdate(CloudMeshItem*)));
thread_controller->runWorker(worker);
-
- return;
}
/////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::SceneTree::slotEstimateNormal()
{
QList<CloudMeshItem*> selected_cloud_mesh_items = selectedTypeItems<CloudMeshItem>();
- AbstractWorker* worker = new NormalEstimationWorker(selected_cloud_mesh_items,&MainWindow::getInstance());
+ AbstractWorker* worker =
+ new NormalEstimationWorker(selected_cloud_mesh_items, &MainWindow::getInstance());
ThreadController* thread_controller = new ThreadController();
- connect(worker, SIGNAL(dataUpdated(CloudMeshItem*)), thread_controller, SLOT(slotOnCloudMeshItemUpdate(CloudMeshItem*)));
+ connect(worker,
+ SIGNAL(dataUpdated(CloudMeshItem*)),
+ thread_controller,
+ SLOT(slotOnCloudMeshItemUpdate(CloudMeshItem*)));
thread_controller->runWorker(worker);
-
- return;
}
/////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::SceneTree::slotPoissonReconstruction()
{
QList<CloudMeshItem*> selected_cloud_mesh_items = selectedTypeItems<CloudMeshItem>();
- AbstractWorker* worker = new PoissonReconstructionWorker(selected_cloud_mesh_items,&MainWindow::getInstance());
+ AbstractWorker* worker = new PoissonReconstructionWorker(selected_cloud_mesh_items,
+ &MainWindow::getInstance());
ThreadController* thread_controller = new ThreadController();
- connect(worker, SIGNAL(dataUpdated(CloudMeshItem*)), thread_controller, SLOT(slotOnCloudMeshItemUpdate(CloudMeshItem*)));
+ connect(worker,
+ SIGNAL(dataUpdated(CloudMeshItem*)),
+ thread_controller,
+ SLOT(slotOnCloudMeshItemUpdate(CloudMeshItem*)));
thread_controller->runWorker(worker);
-
- return;
}
-
/////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::SceneTree::selectRenderWindowItem(RenderWindowItem* render_window_item)
{
selectionModel()->clearSelection();
- selectionModel()->select(indexFromItem(render_window_item), QItemSelectionModel::SelectCurrent);
+ selectionModel()->select(indexFromItem(render_window_item),
+ QItemSelectionModel::SelectCurrent);
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::SceneTree::slotUpdateOnSelectionChange(const QItemSelection & selected, const QItemSelection & deselected)
+pcl::modeler::SceneTree::slotUpdateOnSelectionChange(const QItemSelection& selected,
+ const QItemSelection& deselected)
{
QModelIndexList selected_indices = selected.indexes();
for (QModelIndexList::const_iterator selected_indices_it = selected_indices.begin();
- selected_indices_it != selected_indices.end();
- ++ selected_indices_it)
- {
+ selected_indices_it != selected_indices.end();
+ ++selected_indices_it) {
QTreeWidgetItem* item = itemFromIndex(*selected_indices_it);
RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(item);
- if (render_window_item != nullptr)
- {
+ if (render_window_item != nullptr) {
render_window_item->getRenderWindow()->setActive(true);
}
}
QModelIndexList deselected_indices = deselected.indexes();
- for (QModelIndexList::const_iterator deselected_indices_it = deselected_indices.begin();
- deselected_indices_it != deselected_indices.end();
- ++ deselected_indices_it)
- {
+ for (QModelIndexList::const_iterator deselected_indices_it =
+ deselected_indices.begin();
+ deselected_indices_it != deselected_indices.end();
+ ++deselected_indices_it) {
QTreeWidgetItem* item = itemFromIndex(*deselected_indices_it);
RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(item);
- if (render_window_item != nullptr)
- {
+ if (render_window_item != nullptr) {
render_window_item->getRenderWindow()->setActive(false);
}
}
-
- return;
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::SceneTree::slotUpdateOnInsertOrRemove()
{
- for (int i = 0, i_end = topLevelItemCount(); i < i_end; ++ i)
- {
- RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(topLevelItem(i));
+ for (int i = 0, i_end = topLevelItemCount(); i < i_end; ++i) {
+ RenderWindowItem* render_window_item =
+ dynamic_cast<RenderWindowItem*>(topLevelItem(i));
if (render_window_item == nullptr)
continue;
- QString title = (i == 0)?("Central Render Window"):(QString("Render Window #%1").arg(i));
+ QString title =
+ (i == 0) ? ("Central Render Window") : (QString("Render Window #%1").arg(i));
render_window_item->setText(0, title);
render_window_item->getRenderWindow()->setTitle(title);
QTreeWidget::addTopLevelItem(render_window_item);
selectionModel()->clearSelection();
- selectionModel()->select(indexFromItem(render_window_item), QItemSelectionModel::SelectCurrent);
+ selectionModel()->select(indexFromItem(render_window_item),
+ QItemSelectionModel::SelectCurrent);
emit itemInsertedOrRemoved();
}
void
pcl::modeler::SceneTree::slotCloseRenderWindow()
{
- QList<RenderWindowItem*> selected_render_window_items = selectedTypeItems<RenderWindowItem>();
+ QList<RenderWindowItem*> selected_render_window_items =
+ selectedTypeItems<RenderWindowItem>();
- for (auto &selected_render_window_item : selected_render_window_items)
- {
+ for (auto& selected_render_window_item : selected_render_window_items) {
removeItemWidget(selected_render_window_item, 0);
delete selected_render_window_item;
}
emit itemInsertedOrRemoved();
-
- return;
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::SceneTree::dropEvent(QDropEvent * event)
+pcl::modeler::SceneTree::dropEvent(QDropEvent* event)
{
QList<CloudMeshItem*> selected_cloud_meshes = selectedTypeItems<CloudMeshItem>();
std::set<RenderWindowItem*> previous_parents;
- for (const auto &cloud_mesh_item : selected_cloud_meshes)
- {
- RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(cloud_mesh_item->parent());
+ for (const auto& cloud_mesh_item : selected_cloud_meshes) {
+ RenderWindowItem* render_window_item =
+ dynamic_cast<RenderWindowItem*>(cloud_mesh_item->parent());
if (render_window_item != nullptr)
previous_parents.insert(render_window_item);
}
QTreeWidget::dropEvent(event);
std::vector<CloudMeshItem*> cloud_mesh_items;
- for (const auto &cloud_mesh_item : selected_cloud_meshes)
- {
+ for (const auto& cloud_mesh_item : selected_cloud_meshes) {
if (dynamic_cast<RenderWindowItem*>(cloud_mesh_item->parent()) == nullptr)
cloud_mesh_items.push_back(cloud_mesh_item);
else
}
// put the cloud mesh items in a new render window
- if (!cloud_mesh_items.empty())
- {
- for (const auto &cloud_mesh_item : cloud_mesh_items)
+ if (!cloud_mesh_items.empty()) {
+ for (const auto& cloud_mesh_item : cloud_mesh_items)
takeTopLevelItem(indexFromItem(cloud_mesh_item).row());
- RenderWindowItem* render_window_item = MainWindow::getInstance().createRenderWindow();
- for (const auto &cloud_mesh_item : cloud_mesh_items)
+ RenderWindowItem* render_window_item =
+ MainWindow::getInstance().createRenderWindow();
+ for (const auto& cloud_mesh_item : cloud_mesh_items)
render_window_item->addChild(cloud_mesh_item);
render_window_item->setExpanded(true);
}
- for (const auto &previous_parent : previous_parents)
- {
+ for (const auto& previous_parent : previous_parents) {
previous_parent->getRenderWindow()->updateAxes();
previous_parent->getRenderWindow()->render();
}
-
- return;
}
/////////////////////////////////////////////////////////////////////////////////////////////
bool
-pcl::modeler::SceneTree::dropMimeData(QTreeWidgetItem * parent, int, const QMimeData *, Qt::DropAction)
+pcl::modeler::SceneTree::dropMimeData(QTreeWidgetItem* parent,
+ int,
+ const QMimeData*,
+ Qt::DropAction)
{
QList<CloudMeshItem*> selected_cloud_meshes = selectedTypeItems<CloudMeshItem>();
RenderWindowItem* render_window_item =
- (parent == nullptr)?(MainWindow::getInstance().createRenderWindow()):(dynamic_cast<RenderWindowItem*>(parent));
+ (parent == nullptr) ? (MainWindow::getInstance().createRenderWindow())
+ : (dynamic_cast<RenderWindowItem*>(parent));
- for (auto &selected_cloud_mesh : selected_cloud_meshes)
- {
- CloudMeshItem* cloud_mesh_item_copy = new CloudMeshItem(render_window_item, *selected_cloud_mesh);
+ for (auto& selected_cloud_mesh : selected_cloud_meshes) {
+ CloudMeshItem* cloud_mesh_item_copy =
+ new CloudMeshItem(render_window_item, *selected_cloud_mesh);
render_window_item->addChild(cloud_mesh_item_copy);
setCurrentItem(cloud_mesh_item_copy);
}
*
*/
-#include <pcl/apps/modeler/statistical_outlier_removal_worker.h>
-#include <pcl/apps/modeler/parameter.h>
-#include <pcl/apps/modeler/parameter_dialog.h>
#include <pcl/apps/modeler/cloud_mesh.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
+#include <pcl/apps/modeler/parameter.h>
+#include <pcl/apps/modeler/parameter_dialog.h>
+#include <pcl/apps/modeler/statistical_outlier_removal_worker.h>
#include <pcl/filters/statistical_outlier_removal.h>
-
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::StatisticalOutlierRemovalWorker::StatisticalOutlierRemovalWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent) :
- AbstractWorker(cloud_mesh_items, parent),
- mean_k_(nullptr), stddev_mul_thresh_(nullptr)
-{
-}
+pcl::modeler::StatisticalOutlierRemovalWorker::StatisticalOutlierRemovalWorker(
+ const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent)
+: AbstractWorker(cloud_mesh_items, parent)
+, mean_k_(nullptr)
+, stddev_mul_thresh_(nullptr)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::StatisticalOutlierRemovalWorker::~StatisticalOutlierRemovalWorker()
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::StatisticalOutlierRemovalWorker::initParameters(CloudMeshItem *)
-{
- return;
-}
+pcl::modeler::StatisticalOutlierRemovalWorker::initParameters(CloudMeshItem*)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::StatisticalOutlierRemovalWorker::setupParameters()
{
- mean_k_ = new IntParameter("Mean K", "The number of nearest neighbors to use for mean distance estimation", 8, 1, 1024, 1);
- stddev_mul_thresh_ = new DoubleParameter("Standard Deviation Multiplier", "The distance threshold will be equal to: mean + stddev_mult * stddev. Points will be classified as inlier or outlier if their average neighbor distance is below or above this threshold respectively.", 1.0, 0.1, 10, 0.1);
+ // clang-format off
+ mean_k_ = new IntParameter(
+ "Mean K",
+ "The number of nearest neighbors to use for mean distance estimation",
+ 8, 1, 1024, 1);
+
+ stddev_mul_thresh_ = new DoubleParameter(
+ "Standard Deviation Multiplier",
+ "The distance threshold will be equal to: mean + stddev_mult * stddev. Points "
+ "will be classified as inlier or outlier if their average neighbor distance is "
+ "below or above this threshold respectively.",
+ 1.0, 0.1, 10, 0.1);
+ // clang-format on
parameter_dialog_->addParameter(mean_k_);
parameter_dialog_->addParameter(stddev_mul_thresh_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::StatisticalOutlierRemovalWorker::processImpl(CloudMeshItem* cloud_mesh_item)
+pcl::modeler::StatisticalOutlierRemovalWorker::processImpl(
+ CloudMeshItem* cloud_mesh_item)
{
pcl::StatisticalOutlierRemoval<pcl::PointSurfel> sor;
sor.setInputCloud(cloud_mesh_item->getCloudMesh()->getCloud());
cloud_mesh_item->getCloudMesh()->getCloud() = cloud;
emitDataUpdated(cloud_mesh_item);
-
- return;
}
*
*/
-#include <pcl/apps/modeler/surface_actor_item.h>
-
#include <pcl/apps/modeler/cloud_mesh.h>
+#include <pcl/apps/modeler/surface_actor_item.h>
-#include <vtkLODActor.h>
-#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkDataSetMapper.h>
+#include <vtkLODActor.h>
#include <vtkPointData.h>
+#include <vtkPolyData.h>
#include <vtkProperty.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::SurfaceActorItem::SurfaceActorItem(QTreeWidgetItem* parent,
- const CloudMesh::Ptr& cloud_mesh,
- const vtkSmartPointer<vtkRenderWindow>& render_window)
- :ChannelActorItem(parent, cloud_mesh, render_window, vtkSmartPointer<vtkLODActor>::New(), "Surface")
-{
-}
+pcl::modeler::SurfaceActorItem::SurfaceActorItem(
+ QTreeWidgetItem* parent,
+ const CloudMesh::Ptr& cloud_mesh,
+ const vtkSmartPointer<vtkRenderWindow>& render_window)
+: ChannelActorItem(
+ parent, cloud_mesh, render_window, vtkSmartPointer<vtkLODActor>::New(), "Surface")
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::SurfaceActorItem::~SurfaceActorItem ()
-{
-
-}
+pcl::modeler::SurfaceActorItem::~SurfaceActorItem() {}
//////////////////////////////////////////////////////////////////////////////////////////////
void
vtkSmartPointer<vtkDataArray> scalars;
cloud_mesh_->getColorScalarsFromField(scalars, color_scheme_);
- poly_data_->GetPointData ()->SetScalars (scalars);
+ poly_data_->GetPointData()->SetScalars(scalars);
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
- mapper->SetInputData (poly_data_);
+ mapper->SetInputData(poly_data_);
double minmax[2];
scalars->GetRange(minmax);
mapper->SetScalarRange(minmax);
- mapper->SetScalarModeToUsePointData ();
- mapper->InterpolateScalarsBeforeMappingOn ();
- mapper->ScalarVisibilityOn ();
+ mapper->SetScalarModeToUsePointData();
+ mapper->InterpolateScalarsBeforeMappingOn();
+ mapper->ScalarVisibilityOn();
#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
- mapper->ImmediateModeRenderingOff ();
+ mapper->ImmediateModeRenderingOff();
#endif
- vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>(dynamic_cast<vtkLODActor*>(actor_.GetPointer()));
+ vtkSmartPointer<vtkLODActor> actor =
+ vtkSmartPointer<vtkLODActor>(dynamic_cast<vtkLODActor*>(actor_.GetPointer()));
actor->SetMapper(mapper);
- actor->SetNumberOfCloudPoints(int(std::max<vtkIdType> (1, poly_data_->GetNumberOfPoints () / 10)));
- actor->GetProperty ()->SetInterpolationToFlat ();
+ actor->SetNumberOfCloudPoints(
+ int(std::max<vtkIdType>(1, poly_data_->GetNumberOfPoints() / 10)));
+ actor->GetProperty()->SetInterpolationToFlat();
- actor->GetProperty ()->SetRepresentationToSurface ();
- actor->GetProperty ()->BackfaceCullingOn ();
- actor->GetProperty ()->EdgeVisibilityOff ();
- actor->GetProperty ()->ShadingOff ();
-
- return;
+ actor->GetProperty()->SetRepresentationToSurface();
+ actor->GetProperty()->BackfaceCullingOn();
+ actor->GetProperty()->EdgeVisibilityOff();
+ actor->GetProperty()->ShadingOff();
}
//////////////////////////////////////////////////////////////////////////////////////////////
{
vtkSmartPointer<vtkDataArray> scalars;
cloud_mesh_->getColorScalarsFromField(scalars, "random");
- poly_data_->GetPointData ()->SetScalars (scalars);
+ poly_data_->GetPointData()->SetScalars(scalars);
double minmax[2];
scalars->GetRange(minmax);
actor_->GetMapper()->SetScalarRange(minmax);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::SurfaceActorItem::prepareContextMenu(QMenu *) const
-{
-
-}
+pcl::modeler::SurfaceActorItem::prepareContextMenu(QMenu*) const
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::SurfaceActorItem::prepareProperties(ParameterDialog *)
-{
-
-}
+pcl::modeler::SurfaceActorItem::prepareProperties(ParameterDialog*)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::modeler::SurfaceActorItem::setProperties()
-{
-
-}
+{}
*
*/
-#include <pcl/apps/modeler/thread_controller.h>
-
#include <pcl/apps/modeler/abstract_worker.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
#include <pcl/apps/modeler/main_window.h>
+#include <pcl/apps/modeler/thread_controller.h>
#include <QDialog>
#include <QThread>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::ThreadController::ThreadController()
-{
-
-}
+pcl::modeler::ThreadController::ThreadController() {}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::ThreadController::~ThreadController()
bool
pcl::modeler::ThreadController::runWorker(AbstractWorker* worker)
{
- if (worker->exec() != QDialog::Accepted)
- {
+ if (worker->exec() != QDialog::Accepted) {
delete worker;
deleteLater();
- return (false);
+ return false;
}
-
+
QThread* thread = new QThread;
connect(this, SIGNAL(prepared()), worker, SLOT(process()));
emit prepared();
- return (true);
+ return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::modeler::ThreadController::slotOnCloudMeshItemUpdate(CloudMeshItem* cloud_mesh_item)
+pcl::modeler::ThreadController::slotOnCloudMeshItemUpdate(
+ CloudMeshItem* cloud_mesh_item)
{
cloud_mesh_item->updateChannels();
}
*
*/
-#include <pcl/apps/modeler/voxel_grid_downsample_worker.h>
-#include <pcl/apps/modeler/parameter.h>
-#include <pcl/apps/modeler/parameter_dialog.h>
#include <pcl/apps/modeler/cloud_mesh.h>
#include <pcl/apps/modeler/cloud_mesh_item.h>
-#include <pcl/filters/voxel_grid.h>
+#include <pcl/apps/modeler/parameter.h>
+#include <pcl/apps/modeler/parameter_dialog.h>
+#include <pcl/apps/modeler/voxel_grid_downsample_worker.h>
#include <pcl/common/common.h>
-
+#include <pcl/filters/voxel_grid.h>
//////////////////////////////////////////////////////////////////////////////////////////////
-pcl::modeler::VoxelGridDownampleWorker::VoxelGridDownampleWorker(const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent) :
- AbstractWorker(cloud_mesh_items, parent),
- x_min_(std::numeric_limits<double>::max()), x_max_(std::numeric_limits<double>::min()),
- y_min_(std::numeric_limits<double>::max()), y_max_(std::numeric_limits<double>::min()),
- z_min_(std::numeric_limits<double>::max()), z_max_(std::numeric_limits<double>::min()),
- leaf_size_x_(nullptr), leaf_size_y_(nullptr), leaf_size_z_(nullptr)
-{
-}
+pcl::modeler::VoxelGridDownampleWorker::VoxelGridDownampleWorker(
+ const QList<CloudMeshItem*>& cloud_mesh_items, QWidget* parent)
+: AbstractWorker(cloud_mesh_items, parent)
+, x_min_(std::numeric_limits<double>::max())
+, x_max_(std::numeric_limits<double>::min())
+, y_min_(std::numeric_limits<double>::max())
+, y_max_(std::numeric_limits<double>::min())
+, z_min_(std::numeric_limits<double>::max())
+, z_max_(std::numeric_limits<double>::min())
+, leaf_size_x_(nullptr)
+, leaf_size_y_(nullptr)
+, leaf_size_z_(nullptr)
+{}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::modeler::VoxelGridDownampleWorker::~VoxelGridDownampleWorker()
z_min_ = std::min(double(min_pt.z()), z_min_);
z_max_ = std::max(double(max_pt.z()), z_max_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
double z_range = z_max_ - z_min_;
double range_max = std::max(x_range, std::max(y_range, z_range));
- double size = range_max/1000;
- double step = range_max/1000;
+ double size = range_max / 1000;
+ double step = range_max / 1000;
- leaf_size_x_ = new DoubleParameter("Leaf Size X", "The X size of the voxel grid", size, 0, x_max_-x_min_, step);
- leaf_size_y_ = new DoubleParameter("Leaf Size Y", "The Y size of the voxel grid", size, 0, y_max_-y_min_, step);
- leaf_size_z_ = new DoubleParameter("Leaf Size Z", "The Z size of the voxel grid", size, 0, z_max_-z_min_, step);
+ leaf_size_x_ = new DoubleParameter(
+ "Leaf Size X", "The X size of the voxel grid", size, 0, x_max_ - x_min_, step);
+ leaf_size_y_ = new DoubleParameter(
+ "Leaf Size Y", "The Y size of the voxel grid", size, 0, y_max_ - y_min_, step);
+ leaf_size_z_ = new DoubleParameter(
+ "Leaf Size Z", "The Z size of the voxel grid", size, 0, z_max_ - z_min_, step);
parameter_dialog_->addParameter(leaf_size_x_);
parameter_dialog_->addParameter(leaf_size_y_);
parameter_dialog_->addParameter(leaf_size_z_);
-
- return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
{
pcl::VoxelGrid<pcl::PointSurfel> voxel_grid;
voxel_grid.setInputCloud(cloud_mesh_item->getCloudMesh()->getCloud());
- voxel_grid.setLeafSize (float (*leaf_size_x_), float (*leaf_size_y_), float (*leaf_size_z_));
+ voxel_grid.setLeafSize(
+ float(*leaf_size_x_), float(*leaf_size_y_), float(*leaf_size_z_));
CloudMesh::PointCloudPtr cloud(new CloudMesh::PointCloud());
voxel_grid.filter(*cloud);
cloud_mesh_item->getCloudMesh()->getCloud() = cloud;
emitDataUpdated(cloud_mesh_item);
-
- return;
}
#include <QtGui/QColor>
#include <pcl/apps/point_cloud_editor/localTypes.h>
#include <pcl/apps/point_cloud_editor/statistics.h>
+
+#include <pcl/memory.h> // for pcl::weak_ptr
+
#ifdef OPENGL_IS_A_FRAMEWORK
# include <OpenGL/gl.h>
# include <OpenGL/glu.h>
class Cloud : public Statistics
{
public:
+ /// The type for shared pointer pointing to a selection buffer
+ using SelectionPtr = pcl::shared_ptr<Selection>;
+
+ /// The type for weak pointer pointing to a selection buffer
+ using SelectionWeakPtr = pcl::weak_ptr<Selection>;
+
/// @brief Default Constructor
Cloud ();
/// @brief A weak pointer pointing to the selection object.
/// @details This implementation uses the weak pointer to allow for a lazy
/// update of the cloud if the selection object is destroyed.
- std::weak_ptr<Selection> selection_wk_ptr_;
+ SelectionWeakPtr selection_wk_ptr_;
/// Flag that indicates whether a color ramp should be used (true) or not
/// (false) when displaying the cloud
#include <pcl/apps/point_cloud_editor/statisticsDialog.h>
#include <pcl/apps/point_cloud_editor/toolInterface.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
#include <QGLWidget>
#include <functional>
+class Selection;
+
/// @brief class declaration for the widget for editing and viewing
/// point clouds.
class CloudEditorWidget : public QGLWidget
{
Q_OBJECT
public:
+ /// The type for shared pointer pointing to a selection buffer
+ using SelectionPtr = pcl::shared_ptr<Selection>;
+
/// @brief Constructor
/// @param parent a pointer which points to the parent widget
CloudEditorWidget (QWidget *parent = nullptr);
/// generate scale matrix
void
- getScaleMatrix (int dy, float* matrix);
+ getScaleMatrix (int dy, float* matrix) const;
/// the transform matrix to be used for updating the coordinates of all
/// the points in the cloud
#include <pcl/apps/point_cloud_editor/localTypes.h>
#include <pcl/apps/point_cloud_editor/copyBuffer.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
+class Selection;
+
class CopyCommand : public Command
{
public:
+ /// The type for shared pointer pointing to a constant selection buffer
+ using ConstSelectionPtr = pcl::shared_ptr<const Selection>;
+
/// @brief Constructor
/// @param copy_buffer_ptr a shared pointer pointing to the copy buffer.
/// @param selection_ptr a shared pointer pointing to the selection object.
#include <pcl/apps/point_cloud_editor/copyBuffer.h>
#include <pcl/apps/point_cloud_editor/selection.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
class CutCommand : public Command
{
public:
+ /// The type for shared pointer pointing to a selection buffer
+ using SelectionPtr = pcl::shared_ptr<Selection>;
+
/// @brief Constructor
/// @param copy_buffer_ptr a shared pointer pointing to the copy buffer.
/// @param selection_ptr a shared pointer pointing to the selection object.
#include <pcl/apps/point_cloud_editor/copyBuffer.h>
#include <pcl/apps/point_cloud_editor/selection.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
class DeleteCommand : public Command
{
public:
+ /// The type for shared pointer pointing to a selection buffer
+ using SelectionPtr = pcl::shared_ptr<Selection>;
+
/// @brief Constructor
/// @param selection_ptr A shared pointer pointing to the selection object.
/// @param cloud_ptr A shared pointer pointing to the cloud object.
#include <pcl/apps/point_cloud_editor/selection.h>
#include <pcl/apps/point_cloud_editor/copyBuffer.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
class DenoiseCommand : public Command
{
public:
+ /// The type for shared pointer pointing to a selection buffer
+ using SelectionPtr = pcl::shared_ptr<Selection>;
+
/// @brief Constructor
/// @param selection_ptr a shared pointer pointing to the selection object.
/// @param cloud_ptr a shared pointer pointing to the cloud object.
#include <pcl/apps/point_cloud_editor/command.h>
#include <pcl/apps/point_cloud_editor/localTypes.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
class PasteCommand : public Command
{
public:
+ /// The type for shared pointer pointing to a selection buffer
+ using SelectionPtr = pcl::shared_ptr<Selection>;
+
/// @brief Constructor
/// @param copy_buffer_ptr a shared pointer pointing to the copy buffer.
/// @param selection_ptr a shared pointer pointing to the selection object.
#include <pcl/apps/point_cloud_editor/toolInterface.h>
#include <pcl/apps/point_cloud_editor/localTypes.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
+class Selection;
+
class Select1DTool : public ToolInterface
{
public:
+ /// The type for shared pointer pointing to a selection buffer
+ using SelectionPtr = pcl::shared_ptr<Selection>;
+
/// @brief Constructor
/// @param selection_ptr a shared pointer pointing to the selection object.
/// @param cloud_ptr a shared pointer pointing to the cloud object.
#include <pcl/apps/point_cloud_editor/toolInterface.h>
#include <pcl/apps/point_cloud_editor/localTypes.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
+class Selection;
+
class Select2DTool : public ToolInterface
{
public:
+ /// The type for shared pointer pointing to a selection buffer
+ using SelectionPtr = pcl::shared_ptr<Selection>;
+
/// @brief Constructor
/// @param selection_ptr a shared pointer pointing to the selection object.
/// @param cloud_ptr a shared pointer pointing to the cloud object.
#include <pcl/apps/point_cloud_editor/localTypes.h>
#include <pcl/apps/point_cloud_editor/trackball.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
+class Selection;
+
/// @brief The selection transform tool computes the transform matrix from
/// mouse input. It then updates the cloud's transform matrix for the
/// selected points so that the transformed and selected points will be
class SelectionTransformTool : public ToolInterface
{
public:
+ /// The type for shared pointer pointing to a constant selection buffer
+ using ConstSelectionPtr = pcl::shared_ptr<const Selection>;
+
/// @brief Constructor
/// @param selection_ptr a shared pointer pointing to the selection object.
/// @param cloud_ptr a shared pointer pointing to the cloud object.
private:
- void getPointFromScreenPoint(int s_x, int s_y, float &x, float &y, float &z);
+ void getPointFromScreenPoint(int s_x, int s_y, float &x, float &y, float &z) const;
/// the quaternion representing the current orientation of the trackball
boost::math::quaternion<float> quat_;
#include <pcl/apps/point_cloud_editor/localTypes.h>
#include <pcl/apps/point_cloud_editor/cloud.h>
+#include <pcl/memory.h> // for pcl::shared_ptr
+
+class Selection;
+
class TransformCommand : public Command
{
public:
+ /// The type for shared pointer pointing to a constant selection buffer
+ using ConstSelectionPtr = pcl::shared_ptr<const Selection>;
+
/// @brief Constructor
/// @param selection_ptr a shared pointer pointing to the selection object.
/// @param cloud_ptr a shared pointer pointing to the cloud object.
}
void
-CloudTransformTool::getScaleMatrix (int dy, float* matrix)
+CloudTransformTool::getScaleMatrix (int dy, float* matrix) const
{
setIdentity(matrix);
float scale = dy > 0 ? scale_factor_ : 1.0 / scale_factor_;
void
TrackBall::getPointFromScreenPoint(int s_x, int s_y,
- float &x, float &y, float &z)
+ float &x, float &y, float &z) const
{
// See http://www.opengl.org/wiki/Trackball for more info
* $Id$
*/
-#include <pcl/point_types.h>
-#include <pcl/io/pcd_io.h>
+#include <pcl/common/time.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/filters/convolution.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/common/time.h>
+#include <pcl/point_types.h>
void
-usage (char ** argv)
+usage(char** argv)
{
+ // clang-format off
pcl::console::print_info ("usage: %s <filename> <-r|-c|-s> [-p <borders policy>] [-t <number of threads>] [-d <distance>]\n\n", argv[0]);
- pcl::console::print_info ("Where options are:\n");
- pcl::console::print_info ("\t\t\t-r convolve rows\n");
- pcl::console::print_info ("\t\t\t-c convolve columns\n");
- pcl::console::print_info ("\t\t\t-s convolve separate\n");
- pcl::console::print_info ("\t\t\t-p borders policy\n");
- pcl::console::print_info ("\t\t\t\t Z zero padding, default\n");
- pcl::console::print_info ("\t\t\t\t D duplicate borders\n");
- pcl::console::print_info ("\t\t\t\t M mirror borders\n");
- pcl::console::print_info ("\t\t\t-t optional, number of threads, default 1\n");
- pcl::console::print_info ("\t\t\t-d optional, distance threshold, default 0.001\n");
+ // clang-format on
+ pcl::console::print_info("Where options are:\n");
+ pcl::console::print_info("\t\t\t-r convolve rows\n");
+ pcl::console::print_info("\t\t\t-c convolve columns\n");
+ pcl::console::print_info("\t\t\t-s convolve separate\n");
+ pcl::console::print_info("\t\t\t-p borders policy\n");
+ pcl::console::print_info("\t\t\t\t Z zero padding, default\n");
+ pcl::console::print_info("\t\t\t\t D duplicate borders\n");
+ pcl::console::print_info("\t\t\t\t M mirror borders\n");
+ pcl::console::print_info("\t\t\t-t optional, number of threads, default 1\n");
+ pcl::console::print_info("\t\t\t-d optional, distance threshold, default 0.001\n");
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
int viewport_source, viewport_convolved = 0;
int direction = -1;
double threshold = 0.001;
pcl::filters::Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB> convolution;
Eigen::ArrayXf gaussian_kernel(5);
- gaussian_kernel << 1.f/16, 1.f/4, 3.f/8, 1.f/4, 1.f/16;
- pcl::console::print_info ("convolution kernel:");
- for (Eigen::Index i = 0; i < gaussian_kernel.size (); ++i)
- pcl::console::print_info (" %f", gaussian_kernel[i]);
- pcl::console::print_info ("\n");
+ gaussian_kernel << 1.f / 16, 1.f / 4, 3.f / 8, 1.f / 4, 1.f / 16;
+ pcl::console::print_info("convolution kernel:");
+ for (Eigen::Index i = 0; i < gaussian_kernel.size(); ++i)
+ pcl::console::print_info(" %f", gaussian_kernel[i]);
+ pcl::console::print_info("\n");
- if (argc < 3)
- {
- usage (argv);
+ if (argc < 3) {
+ usage(argv);
return 1;
}
// check if user is requesting help
- std::string arg (argv[1]);
+ std::string arg(argv[1]);
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
// user don't need help find convolving direction
// convolve row
- if (pcl::console::find_switch (argc, argv, "-r"))
+ if (pcl::console::find_switch(argc, argv, "-r"))
direction = 0;
- else
- {
+ else {
// convolve column
- if (pcl::console::find_switch (argc, argv, "-c"))
+ if (pcl::console::find_switch(argc, argv, "-c"))
direction = 1;
else
- // convolve both
- if (pcl::console::find_switch (argc, argv, "-s"))
- direction = 2;
- else
- {
- // wrong direction given print usage
- usage (argv);
- return 1;
- }
+ // convolve both
+ if (pcl::console::find_switch(argc, argv, "-s"))
+ direction = 2;
+ else {
+ // wrong direction given print usage
+ usage(argv);
+ return 1;
+ }
}
// number of threads if any
- if (pcl::console::parse_argument (argc, argv, "-t", nb_threads) != -1 )
- {
+ if (pcl::console::parse_argument(argc, argv, "-t", nb_threads) != -1) {
if (nb_threads <= 0)
nb_threads = 1;
#ifndef _OPENMP
- if (nb_threads > 1)
- {
- pcl::console::print_info ("OpenMP not activated. Number of threads: 1\n");
+ if (nb_threads > 1) {
+ pcl::console::print_info("OpenMP not activated. Number of threads: 1\n");
nb_threads = 1;
}
#endif
}
#ifdef _OPENMP
- else
- {
+ else {
nb_threads = omp_get_num_procs();
}
#endif
- convolution.setNumberOfThreads (nb_threads);
+ convolution.setNumberOfThreads(nb_threads);
// borders policy if any
- if (pcl::console::parse_argument (argc, argv, "-p", border_policy) != -1 )
- {
- switch (border_policy)
- {
- case 'Z' : convolution.setBordersPolicy (pcl::filters::Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::BORDERS_POLICY_IGNORE);
- break;
- case 'M' : convolution.setBordersPolicy (pcl::filters::Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::BORDERS_POLICY_MIRROR);
- break;
- case 'D' : convolution.setBordersPolicy (pcl::filters::Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::BORDERS_POLICY_DUPLICATE);
- break;
- default :
- {
- usage (argv);
- return (1);
- }
+ if (pcl::console::parse_argument(argc, argv, "-p", border_policy) != -1) {
+ switch (border_policy) {
+ case 'Z':
+ convolution.setBordersPolicy(
+ pcl::filters::Convolution<pcl::PointXYZRGB,
+ pcl::PointXYZRGB>::BORDERS_POLICY_IGNORE);
+ break;
+ case 'M':
+ convolution.setBordersPolicy(
+ pcl::filters::Convolution<pcl::PointXYZRGB,
+ pcl::PointXYZRGB>::BORDERS_POLICY_MIRROR);
+ break;
+ case 'D':
+ convolution.setBordersPolicy(
+ pcl::filters::Convolution<pcl::PointXYZRGB,
+ pcl::PointXYZRGB>::BORDERS_POLICY_DUPLICATE);
+ break;
+ default: {
+ usage(argv);
+ return 1;
+ }
}
}
else
- convolution.setBordersPolicy (pcl::filters::Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::BORDERS_POLICY_IGNORE);
+ convolution.setBordersPolicy(
+ pcl::filters::Convolution<pcl::PointXYZRGB,
+ pcl::PointXYZRGB>::BORDERS_POLICY_IGNORE);
// distance threshold if any
- if (pcl::console::parse_argument (argc, argv, "-d", threshold) == -1 )
- {
+ if (pcl::console::parse_argument(argc, argv, "-d", threshold) == -1) {
threshold = 0.01;
}
- convolution.setDistanceThreshold (static_cast<float> (threshold));
+ convolution.setDistanceThreshold(static_cast<float>(threshold));
// all set
// we have file name and convolving direction
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
- if (pcl::io::loadPCDFile (argv[1], *cloud) == -1)
- {
- pcl::console::print_error ("Couldn't read file %s \n", argv[1]);
- return (-1);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+ if (pcl::io::loadPCDFile(argv[1], *cloud) == -1) {
+ pcl::console::print_error("Couldn't read file %s \n", argv[1]);
+ return -1;
}
cloud->is_dense = false;
- convolution.setInputCloud (cloud);
- convolution.setKernel (gaussian_kernel);
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr convolved (new pcl::PointCloud<pcl::PointXYZRGB> ());
+ convolution.setInputCloud(cloud);
+ convolution.setKernel(gaussian_kernel);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr convolved(
+ new pcl::PointCloud<pcl::PointXYZRGB>());
double t0;
- pcl::console::print_info ("convolving %s along \n", argv[1]);
+ pcl::console::print_info("convolving %s along \n", argv[1]);
std::ostringstream convolved_label;
convolved_label << "convolved along ";
- switch (direction)
- {
- case 0:
- {
- convolved_label << "rows... ";
- t0 = pcl::getTime ();
- convolution.convolveRows (*convolved);
- break;
- }
- case 1:
- {
- convolved_label << "columns... ";
- t0 = pcl::getTime ();
- convolution.convolveCols (*convolved);
- break;
- }
- case 2:
- {
- convolved_label << "rows and columns... ";
- t0 = pcl::getTime ();
- convolution.convolve (*convolved);
- break;
- }
+ switch (direction) {
+ case 0: {
+ convolved_label << "rows... ";
+ t0 = pcl::getTime();
+ convolution.convolveRows(*convolved);
+ break;
+ }
+ case 1: {
+ convolved_label << "columns... ";
+ t0 = pcl::getTime();
+ convolution.convolveCols(*convolved);
+ break;
+ }
+ case 2: {
+ convolved_label << "rows and columns... ";
+ t0 = pcl::getTime();
+ convolution.convolve(*convolved);
+ break;
}
- convolved_label << pcl::getTime () - t0 << "s";
+ }
+ convolved_label << pcl::getTime() - t0 << "s";
#ifdef _OPENMP
convolved_label << "\ncpu cores: " << omp_get_num_procs() << " ";
#else
convolved_label << "\n";
-#endif
+#endif
convolved_label << "threads: " << nb_threads;
// Display
- pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("Convolution"));
+ pcl::visualization::PCLVisualizer::Ptr viewer(
+ new pcl::visualization::PCLVisualizer("Convolution"));
// viewport stuff
- viewer->createViewPort (0, 0, 0.5, 1, viewport_source);
- viewer->createViewPort (0.5, 0, 1, 1, viewport_convolved);
- viewer->setBackgroundColor (0, 0, 0);
+ viewer->createViewPort(0, 0, 0.5, 1, viewport_source);
+ viewer->createViewPort(0.5, 0, 1, 1, viewport_convolved);
+ viewer->setBackgroundColor(0, 0, 0);
// Source
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color_handler_source (cloud);
- viewer->addPointCloud<pcl::PointXYZRGB> (cloud, color_handler_source, "source", viewport_source);
- viewer->addText ("source", 10, 10, "source_label", viewport_source);
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>
+ color_handler_source(cloud);
+ viewer->addPointCloud<pcl::PointXYZRGB>(
+ cloud, color_handler_source, "source", viewport_source);
+ viewer->addText("source", 10, 10, "source_label", viewport_source);
// Convolved
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color_handler_convolved (convolved);
- viewer->addPointCloud<pcl::PointXYZRGB> (convolved, color_handler_convolved, "convolved", viewport_convolved);
- viewer->addText (convolved_label.str (), 10, 10, "convolved_label", viewport_convolved);
- viewer->spin ();
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>
+ color_handler_convolved(convolved);
+ viewer->addPointCloud<pcl::PointXYZRGB>(
+ convolved, color_handler_convolved, "convolved", viewport_convolved);
+ viewer->addText(convolved_label.str(), 10, 10, "convolved_label", viewport_convolved);
+ viewer->spin();
pcl::PCDWriter writer;
- writer.write<pcl::PointXYZRGB> ("convolved.pcd", *convolved, false);
+ writer.write<pcl::PointXYZRGB>("convolved.pcd", *convolved, false);
}
*
*/
-#include <thread>
-
#include <pcl/common/time.h>
-#include <pcl/point_types.h>
#include <pcl/io/dinast_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_types.h>
+
+#include <thread>
using namespace std::chrono_literals;
template <typename PointType>
-class DinastProcessor
-{
- public:
-
- using Cloud = pcl::PointCloud<PointType>;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- DinastProcessor(pcl::Grabber& grabber) : interface(grabber), viewer("Dinast Cloud Viewer") {}
+class DinastProcessor {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudConstPtr = typename Cloud::ConstPtr;
- void
- cloud_cb_ (CloudConstPtr cloud_cb)
- {
- static unsigned count = 0;
- static double last = pcl::getTime ();
- if (++count == 30)
- {
- double now = pcl::getTime ();
- std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl;
- count = 0;
- last = now;
- }
- if (!viewer.wasStopped())
- viewer.showCloud(cloud_cb);
+ DinastProcessor(pcl::Grabber& grabber)
+ : interface(grabber), viewer("Dinast Cloud Viewer")
+ {}
+
+ void
+ cloud_cb_(CloudConstPtr cloud_cb)
+ {
+ static unsigned count = 0;
+ static double last = pcl::getTime();
+ if (++count == 30) {
+ double now = pcl::getTime();
+ std::cout << "Average framerate: " << double(count) / double(now - last) << " Hz"
+ << std::endl;
+ count = 0;
+ last = now;
}
-
- int
- run ()
- {
+ if (!viewer.wasStopped())
+ viewer.showCloud(cloud_cb);
+ }
+
+ int
+ run()
+ {
+
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb_ (cloud); };
+ boost::signals2::connection c = interface.registerCallback(f);
- boost::signals2::connection c = interface.registerCallback (f);
+ interface.start();
- interface.start ();
-
- while (!viewer.wasStopped())
- {
- std::this_thread::sleep_for(1s);
- }
-
- interface.stop ();
-
- return(0);
+ while (!viewer.wasStopped()) {
+ std::this_thread::sleep_for(1s);
}
-
- pcl::Grabber& interface;
- pcl::visualization::CloudViewer viewer;
-
+
+ interface.stop();
+
+ return 0;
+ }
+
+ pcl::Grabber& interface;
+ pcl::visualization::CloudViewer viewer;
};
int
-main ()
+main()
{
pcl::DinastGrabber grabber;
- DinastProcessor<pcl::PointXYZI> v (grabber);
- v.run ();
- return (0);
+ DinastProcessor<pcl::PointXYZI> v(grabber);
+ v.run();
+ return 0;
}
#ifndef PCL_NO_PRECOMPILE
#include <pcl/apps/impl/dominant_plane_segmentation.hpp>
-#include <pcl/point_types.h>
#include <pcl/impl/instantiate.hpp>
+#include <pcl/point_types.h>
#ifdef PCL_ONLY_CORE_POINT_TYPES
- PCL_INSTANTIATE(DominantPlaneSegmentation, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB))
+PCL_INSTANTIATE(DominantPlaneSegmentation,
+ (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB))
#else
- PCL_INSTANTIATE(DominantPlaneSegmentation, PCL_XYZ_POINT_TYPES)
+PCL_INSTANTIATE(DominantPlaneSegmentation, PCL_XYZ_POINT_TYPES)
#endif // PCL_ONLY_CORE_POINT_TYPES
#endif // PCL_NO_PRECOMPILE
* Author: Aitor Aldoma
*/
-#include "pcl/recognition/face_detection/rf_face_detector_trainer.h"
#include <pcl/console/parse.h>
+#include <pcl/recognition/face_detection/rf_face_detector_trainer.h>
-int main(int argc, char ** argv)
+int
+main(int argc, char** argv)
{
int ntrees = 10;
std::string forest_fn = "forest.txt";
int use_normals = 0;
int num_images = 3000;
- pcl::console::parse_argument (argc, argv, "-ntrees", ntrees);
- pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn);
- pcl::console::parse_argument (argc, argv, "-n_features", n_features);
- pcl::console::parse_argument (argc, argv, "-directory", directory);
- pcl::console::parse_argument (argc, argv, "-use_normals", use_normals);
- pcl::console::parse_argument (argc, argv, "-num_images", num_images);
+ pcl::console::parse_argument(argc, argv, "-ntrees", ntrees);
+ pcl::console::parse_argument(argc, argv, "-forest_fn", forest_fn);
+ pcl::console::parse_argument(argc, argv, "-n_features", n_features);
+ pcl::console::parse_argument(argc, argv, "-directory", directory);
+ pcl::console::parse_argument(argc, argv, "-use_normals", use_normals);
+ pcl::console::parse_argument(argc, argv, "-num_images", num_images);
pcl::RFFaceDetectorTrainer fdrf;
- fdrf.setForestFilename (forest_fn);
- fdrf.setDirectory (directory);
- fdrf.setWSize (80);
- fdrf.setNumTrees (ntrees);
- fdrf.setNumFeatures (n_features);
- fdrf.setUseNormals (static_cast<bool> (use_normals));
- fdrf.setNumTrainingImages (num_images);
+ fdrf.setForestFilename(forest_fn);
+ fdrf.setDirectory(directory);
+ fdrf.setWSize(80);
+ fdrf.setNumTrees(ntrees);
+ fdrf.setNumFeatures(n_features);
+ fdrf.setUseNormals(static_cast<bool>(use_normals));
+ fdrf.setNumTrainingImages(num_images);
- //TODO: do some checks here..., fn file exists, directory exists, etc...
- fdrf.trainWithDataProvider ();
+ // TODO: do some checks here..., fn file exists, directory exists, etc...
+ fdrf.trainWithDataProvider();
}
-
* Author: Aitor Aldoma
*/
-#include "pcl/recognition/face_detection/rf_face_detector_trainer.h"
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
+#include <pcl/recognition/face_detection/rf_face_detector_trainer.h>
#include <pcl/visualization/pcl_visualizer.h>
+// clang-format off
+#include <pcl/apps/face_detection/face_detection_apps_utils.h>
+// clang-format on
+
#include <boost/filesystem.hpp>
-namespace bf = boost::filesystem;
-#include "pcl/apps/face_detection/face_detection_apps_utils.h"
+namespace bf = boost::filesystem;
bool SHOW_GT = false;
bool VIDEO = false;
-template<class PointInT>
-void run(pcl::RFFaceDetectorTrainer & fdrf, typename pcl::PointCloud<PointInT>::Ptr & scene_vis, pcl::visualization::PCLVisualizer & vis, bool heat_map,
- bool show_votes, const std::string & filename)
+template <class PointInT>
+void
+run(pcl::RFFaceDetectorTrainer& fdrf,
+ typename pcl::PointCloud<PointInT>::Ptr& scene_vis,
+ pcl::visualization::PCLVisualizer& vis,
+ bool heat_map,
+ bool show_votes,
+ const std::string& filename)
{
- pcl::PointCloud<pcl::PointXYZ>::Ptr scene
(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::copyPointCloud (*scene_vis, *scene);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr scene(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::copyPointCloud(*scene_vis, *scene);
- fdrf.setInputCloud (scene);
+ fdrf.setInputCloud(scene);
- if (heat_map)
- {
-
pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
- fdrf.setFaceHeatMapCloud (intensity_cloud);
+ if (heat_map) {
+ pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud(
+
new pcl::PointCloud<pcl::PointXYZI>);
+ fdrf.setFaceHeatMapCloud(intensity_cloud);
}
- fdrf.detectFaces ();
+ fdrf.detectFaces();
using FieldListM = typename pcl::traits::fieldList<PointInT>::type;
float rgb_m;
bool exists_m;
- pcl::for_each_type < FieldListM > (pcl::CopyIfFieldExists<PointInT, float> (scene_vis->points[0], "rgb", exists_m, rgb_m));
-
- std::cout << "Color exists:" << static_cast<int> (exists_m) << std::endl;
- if (exists_m)
- {
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr to_visualize (new pcl::PointCloud<pcl::PointXYZRGB>);
- pcl::copyPointCloud (*scene_vis, *to_visualize);
-
- pcl::visualization::PointCloudColorHandlerRGBField < pcl::PointXYZRGB > handler_keypoints (to_visualize);
- vis.addPointCloud < pcl::PointXYZRGB > (to_visualize, handler_keypoints, "scene_cloud");
- } else
- {
- vis.addPointCloud (scene_vis, "scene_cloud");
+ pcl::for_each_type<FieldListM>(pcl::CopyIfFieldExists<PointInT, float>(
+ scene_vis->points[0], "rgb", exists_m, rgb_m));
+
+ std::cout << "Color exists:" << static_cast<int>(exists_m) << std::endl;
+ if (exists_m) {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr to_visualize(
+ new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::copyPointCloud(*scene_vis, *to_visualize);
+
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>
+ handler_keypoints(to_visualize);
+ vis.addPointCloud<pcl::PointXYZRGB>(to_visualize, handler_keypoints, "scene_cloud");
+ }
+ else {
+ vis.addPointCloud(scene_vis, "scene_cloud");
}
- if (heat_map)
- {
-
pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
- fdrf.getFaceHeatMap (intensity_cloud);
+ if (heat_map) {
+ pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud(
+
new pcl::PointCloud<pcl::PointXYZI>);
+ fdrf.getFaceHeatMap(intensity_cloud);
- pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity");
- vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map");
+ pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>
+ handler_keypoints(intensity_cloud, "intensity");
+ vis.addPointCloud<pcl::PointXYZI>(intensity_cloud, handler_keypoints, "heat_map");
}
- if (show_votes)
- {
- //display votes_
- /*pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud(new pcl::PointCloud<pcl::PointXYZ>());
- fdrf.getVotes(votes_cloud);
- pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes(votes_cloud, 255, 0, 0);
- vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud");
- vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
- vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud");
- vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");*/
-
- pcl::PointCloud<pcl::PointXYZI>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
- fdrf.getVotes2 (votes_cloud);
- pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_votes (votes_cloud, "intensity");
- vis.addPointCloud < pcl::PointXYZI > (votes_cloud, handler_votes, "votes_cloud");
- vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
+ if (show_votes) {
+
+ pcl::PointCloud<pcl::PointXYZI>::Ptr votes_cloud(
+ new pcl::PointCloud<pcl::PointXYZI>());
+ fdrf.getVotes2(votes_cloud);
+ pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>
+ handler_votes(votes_cloud, "intensity");
+ vis.addPointCloud<pcl::PointXYZI>(votes_cloud, handler_votes, "votes_cloud");
+ vis.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
}
- vis.addCoordinateSystem (0.1, "global");
+ vis.addCoordinateSystem(0.1, "global");
std::vector<Eigen::VectorXf> heads;
- fdrf.getDetectedFaces (heads);
- face_detection_apps_utils::displayHeads (heads, vis);
+ fdrf.getDetectedFaces(heads);
+ face_detection_apps_utils::displayHeads(heads, vis);
- if (SHOW_GT)
- {
- //check if there is ground truth data
- std::string pose_file (filename);
- boost::replace_all (pose_file, ".pcd", "_pose.txt");
+ if (SHOW_GT) {
+ // check if there is ground truth data
+ std::string pose_file(filename);
+ boost::replace_all(pose_file, ".pcd", "_pose.txt");
Eigen::Matrix4f pose_mat;
- pose_mat.setIdentity (4, 4);
- bool result = face_detection_apps_utils::readMatrixFromFile (pose_file, pose_mat);
+ pose_mat.setIdentity(4, 4);
+ bool result = face_detection_apps_utils::readMatrixFromFile(pose_file, pose_mat);
- if (result)
- {
- Eigen::Vector3f ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
- Eigen::Vector3f trans_vector = Eigen::Vector3f (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3));
+ if (result) {
+ Eigen::Vector3f ea = pose_mat.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
+ Eigen::Vector3f trans_vector =
+ Eigen::Vector3f(pose_mat(0, 3), pose_mat(1, 3), pose_mat(2, 3));
std::cout << ea << std::endl;
std::cout << trans_vector << std::endl;
center_point.x = trans_vector[0];
center_point.y = trans_vector[1];
center_point.z = trans_vector[2];
- vis.addSphere (center_point, 0.05, 255, 0, 0, "sphere");
+ vis.addSphere(center_point, 0.05, 255, 0, 0, "sphere");
pcl::ModelCoefficients cylinder_coeff;
- cylinder_coeff.values.resize (7); // We need 7 values
+ cylinder_coeff.values.resize(7); // We need 7 values
cylinder_coeff.values[0] = center_point.x;
cylinder_coeff.values[1] = center_point.y;
cylinder_coeff.values[2] = center_point.z;
cylinder_coeff.values[4] = ea[1];
cylinder_coeff.values[5] = ea[2];
- Eigen::Vector3f vec = Eigen::Vector3f::UnitZ () * -1.f;
+ Eigen::Vector3f vec = Eigen::Vector3f::UnitZ() * -1.f;
Eigen::Matrix3f matrixxx;
- matrixxx = Eigen::AngleAxisf (ea[0], Eigen::Vector3f::UnitX ()) * Eigen::AngleAxisf (ea[1], Eigen::Vector3f::UnitY ())
- * Eigen::AngleAxisf (ea[2], Eigen::Vector3f::UnitZ ());
+ matrixxx = Eigen::AngleAxisf(ea[0], Eigen::Vector3f::UnitX()) *
+ Eigen::AngleAxisf(ea[1], Eigen::Vector3f::UnitY()) *
+ Eigen::AngleAxisf(ea[2], Eigen::Vector3f::UnitZ());
- //matrixxx = pose_mat.block<3,3>(0,0);
+ // matrixxx = pose_mat.block<3,3>(0,0);
vec = matrixxx * vec;
cylinder_coeff.values[3] = vec[0];
cylinder_coeff.values[5] = vec[2];
cylinder_coeff.values[6] = 0.01f;
- vis.addCylinder (cylinder_coeff, "cylinder");
+ vis.addCylinder(cylinder_coeff, "cylinder");
}
}
- vis.setRepresentationToSurfaceForAllActors ();
+ vis.setRepresentationToSurfaceForAllActors();
- if (VIDEO)
- {
- vis.spinOnce (50, true);
- } else
- {
- vis.spin ();
+ if (VIDEO) {
+ vis.spinOnce(50, true);
+ }
+ else {
+ vis.spin();
}
- vis.removeAllPointClouds ();
- vis.removeAllShapes ();
+ vis.removeAllPointClouds();
+ vis.removeAllShapes();
}
-int main(int argc, char ** argv)
+int
+main(int argc, char** argv)
{
int STRIDE_SW = 5;
std::string forest_fn = "forest.txt";
int icp_iterations = 5;
std::string model_path_;
- pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn);
- pcl::console::parse_argument (argc, argv, "-max_variance", trans_max_variance);
- pcl::console::parse_argument (argc, argv, "-min_votes_size", min_votes_size);
- pcl::console::parse_argument (argc, argv, "-use_normals", use_normals);
- pcl::console::parse_argument (argc, argv, "-face_threshold", face_threshold);
- pcl::console::parse_argument (argc, argv, "-stride_sw", STRIDE_SW);
- pcl::console::parse_argument (argc, argv, "-heat_map", heat_map);
- pcl::console::parse_argument (argc, argv, "-show_votes", show_votes);
- pcl::console::parse_argument (argc, argv, "-test_directory", test_directory);
- pcl::console::parse_argument (argc, argv, "-filename", filename);
- pcl::console::parse_argument (argc, argv, "-rgb_exists", rgb_exists);
- pcl::console::parse_argument (argc, argv, "-show_gt", SHOW_GT);
- pcl::console::parse_argument (argc, argv, "-pose_refinement", pose_refinement_);
- pcl::console::parse_argument (argc, argv, "-model_path", model_path_);
- pcl::console::parse_argument (argc, argv, "-icp_iterations", icp_iterations);
- pcl::console::parse_argument (argc, argv, "-video", VIDEO);
+ pcl::console::parse_argument(argc, argv, "-forest_fn", forest_fn);
+ pcl::console::parse_argument(argc, argv, "-max_variance", trans_max_variance);
+ pcl::console::parse_argument(argc, argv, "-min_votes_size", min_votes_size);
+ pcl::console::parse_argument(argc, argv, "-use_normals", use_normals);
+ pcl::console::parse_argument(argc, argv, "-face_threshold", face_threshold);
+ pcl::console::parse_argument(argc, argv, "-stride_sw", STRIDE_SW);
+ pcl::console::parse_argument(argc, argv, "-heat_map", heat_map);
+ pcl::console::parse_argument(argc, argv, "-show_votes", show_votes);
+ pcl::console::parse_argument(argc, argv, "-test_directory", test_directory);
+ pcl::console::parse_argument(argc, argv, "-filename", filename);
+ pcl::console::parse_argument(argc, argv, "-rgb_exists", rgb_exists);
+ pcl::console::parse_argument(argc, argv, "-show_gt", SHOW_GT);
+ pcl::console::parse_argument(argc, argv, "-pose_refinement", pose_refinement_);
+ pcl::console::parse_argument(argc, argv, "-model_path", model_path_);
+ pcl::console::parse_argument(argc, argv, "-icp_iterations", icp_iterations);
+ pcl::console::parse_argument(argc, argv, "-video", VIDEO);
pcl::RFFaceDetectorTrainer fdrf;
- fdrf.setForestFilename (forest_fn);
- fdrf.setWSize (80);
- fdrf.setUseNormals (static_cast<bool> (use_normals));
- fdrf.setWStride (STRIDE_SW);
- fdrf.setLeavesFaceMaxVariance (trans_max_variance);
- fdrf.setLeavesFaceThreshold (face_threshold);
- fdrf.setFaceMinVotes (min_votes_size);
-
- if (pose_refinement_)
- {
- fdrf.setPoseRefinement (true, icp_iterations);
- fdrf.setModelPath (model_path_);
+ fdrf.setForestFilename(forest_fn);
+ fdrf.setWSize(80);
+ fdrf.setUseNormals(static_cast<bool>(use_normals));
+ fdrf.setWStride(STRIDE_SW);
+ fdrf.setLeavesFaceMaxVariance(trans_max_variance);
+ fdrf.setLeavesFaceThreshold(face_threshold);
+ fdrf.setFaceMinVotes(min_votes_size);
+
+ if (pose_refinement_) {
+ fdrf.setPoseRefinement(true, icp_iterations);
+ fdrf.setModelPath(model_path_);
}
-//load forest from file and pass it to the detector
+ // load forest from file and pass it to the detector
std::filebuf fb;
- fb.open (forest_fn.c_str (), std::ios::in);
- std::istream os (&fb);
+ fb.open(forest_fn.c_str(), std::ios::in);
+ std::istream os(&fb);
using NodeType = pcl::face_detection::RFTreeNode<pcl::face_detection::FeatureType>;
pcl::DecisionForest<NodeType> forest;
- forest.deserialize (os);
- fb.close ();
+ forest.deserialize(os);
+ fb.close();
- fdrf.setForest (forest);
+ fdrf.setForest(forest);
- pcl::visualization::PCLVisualizer vis ("PCL Face detection");
+ pcl::visualization::PCLVisualizer vis("PCL Face detection");
- if (!test_directory.empty())
- {
- //recognize all files in directory...
+ if (!test_directory.empty()) {
+ // recognize all files in directory...
std::string start;
- std::string ext = std::string ("pcd");
+ std::string ext = std::string("pcd");
bf::path dir = test_directory;
std::vector<std::string> files;
- face_detection_apps_utils::getFilesInDirectory (dir, start, files, ext);
+ face_detection_apps_utils::getFilesInDirectory(dir, start, files, ext);
- std::sort (files.begin (), files.end (), face_detection_apps_utils::sortFiles);
+ std::sort(files.begin(), files.end(), face_detection_apps_utils::sortFiles);
- for (const auto &filename : files)
- {
+ for (const auto& filename : files) {
std::string file = test_directory + '/' + filename;
std::cout << file << std::endl;
- if (rgb_exists)
- {
+ if (rgb_exists) {
std::cout << "Loading a PointXYZRGBA cloud..." << std::endl;
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
- pcl::io::loadPCDFile (file, *cloud);
- run<pcl::PointXYZRGB> (fdrf, cloud, vis, heat_map, show_votes, file);
- } else
- {
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
- pcl::io::loadPCDFile (file, *cloud);
- run<pcl::PointXYZ> (fdrf, cloud, vis, heat_map, show_votes, file);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
+ new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::io::loadPCDFile(file, *cloud);
+ run<pcl::PointXYZRGB>(fdrf, cloud, vis, heat_map, show_votes, file);
+ }
+ else {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::io::loadPCDFile(file, *cloud);
+ run<pcl::PointXYZ>(fdrf, cloud, vis, heat_map, show_votes, file);
}
}
- } else
- {
- if (rgb_exists)
- {
-
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
- pcl::io::loadPCDFile (filename, *cloud);
- run<pcl::PointXYZRGB> (fdrf, cloud, vis, heat_map, show_votes, filename);
- } else
- {
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::io::loadPCDFile (filename, *cloud);
- run<pcl::PointXYZ> (fdrf, cloud, vis, heat_map, show_votes, filename);
+ }
+ else {
+ if (rgb_exists) {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
+
new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::io::loadPCDFile(filename, *cloud);
+ run<pcl::PointXYZRGB>(fdrf, cloud, vis, heat_map, show_votes, filename);
+ }
+ else {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::io::loadPCDFile(filename, *cloud);
+ run<pcl::PointXYZ>(fdrf, cloud, vis, heat_map, show_votes, filename);
}
}
}
-
* Author: Aitor Aldoma
*/
-#include "pcl/recognition/face_detection/rf_face_detector_trainer.h"
-#include "pcl/apps/face_detection/openni_frame_source.h"
-#include "pcl/apps/face_detection/face_detection_apps_utils.h"
+#include <pcl/apps/face_detection/openni_frame_source.h>
#include <pcl/common/time.h>
#include <pcl/console/parse.h>
#include <pcl/features/integral_image_normal.h>
+#include <pcl/recognition/face_detection/rf_face_detector_trainer.h>
+// clang-format off
+#include <pcl/apps/face_detection/face_detection_apps_utils.h>
+// clang-format on
-void run(pcl::RFFaceDetectorTrainer & fdrf, bool heat_map = false, bool show_votes = false)
+void
+run(pcl::RFFaceDetectorTrainer& fdrf, bool heat_map = false, bool show_votes = false)
{
OpenNIFrameSource::OpenNIFrameSource camera;
OpenNIFrameSource::PointCloudPtr scene_vis;
- pcl::visualization::PCLVisualizer vis ("Face dection");
- vis.addCoordinateSystem (0.1, "global");
+ pcl::visualization::PCLVisualizer vis("Face dection");
+ vis.addCoordinateSystem(0.1, "global");
- //keyboard callback to stop getting frames and finalize application
- std::function<void(const pcl::visualization::KeyboardEvent&)> keyboard_cb = [&] (const pcl::visualization::KeyboardEvent& event)
- {
- camera.onKeyboardEvent (event);
- };
- vis.registerKeyboardCallback (keyboard_cb);
+ // keyboard callback to stop getting frames and finalize application
+ std::function<void(const pcl::visualization::KeyboardEvent&)> keyboard_cb =
+ [&](const pcl::visualization::KeyboardEvent& event) {
+ camera.onKeyboardEvent(event);
+ };
+ vis.registerKeyboardCallback(keyboard_cb);
- while (camera.isActive ())
- {
- scene_vis = camera.snap ();
+ while (camera.isActive()) {
+ scene_vis = camera.snap();
- pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ> ());
- pcl::copyPointCloud (*scene_vis, *scene);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr scene(new pcl::PointCloud<pcl::PointXYZ>());
+ pcl::copyPointCloud(*scene_vis, *scene);
- fdrf.setInputCloud (scene);
+ fdrf.setInputCloud(scene);
- if (heat_map)
- {
-
pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
- fdrf.setFaceHeatMapCloud (intensity_cloud);
+ if (heat_map) {
+ pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud(
+
new pcl::PointCloud<pcl::PointXYZI>);
+ fdrf.setFaceHeatMapCloud(intensity_cloud);
}
{
- pcl::ScopeTime t ("Detect faces...");
- fdrf.detectFaces ();
+ pcl::ScopeTime t("Detect faces...");
+ fdrf.detectFaces();
}
- pcl::visualization::PointCloudColorHandlerRGBField<OpenNIFrameSource::PointT> handler_keypoints (scene_vis);
- vis.addPointCloud < OpenNIFrameSource::PointT > (scene_vis, handler_keypoints, "scene_cloud");
-
- if (heat_map)
- {
- pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
- fdrf.getFaceHeatMap (intensity_cloud);
- pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity");
- vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map");
+ pcl::visualization::PointCloudColorHandlerRGBField<OpenNIFrameSource::PointT>
+ handler_keypoints(scene_vis);
+ vis.addPointCloud<OpenNIFrameSource::PointT>(
+ scene_vis, handler_keypoints, "scene_cloud");
+
+ if (heat_map) {
+ pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud(
+ new pcl::PointCloud<pcl::PointXYZI>);
+ fdrf.getFaceHeatMap(intensity_cloud);
+ pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>
+ handler_keypoints(intensity_cloud, "intensity");
+ vis.addPointCloud<pcl::PointXYZI>(intensity_cloud, handler_keypoints, "heat_map");
}
- if (show_votes)
- {
- //display votes_
- pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
- fdrf.getVotes (votes_cloud);
- pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes (votes_cloud, 255, 0, 0);
- vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud");
- vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
- vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud");
- vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");
+ if (show_votes) {
+ // display votes_
+ pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud(
+ new pcl::PointCloud<pcl::PointXYZ>());
+ fdrf.getVotes(votes_cloud);
+ pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_votes(
+ votes_cloud, 255, 0, 0);
+ vis.addPointCloud<pcl::PointXYZ>(votes_cloud, handler_votes, "votes_cloud");
+ vis.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
+ vis.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud");
+ vis.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");
}
std::vector<Eigen::VectorXf> heads;
- fdrf.getDetectedFaces (heads);
+ fdrf.getDetectedFaces(heads);
- face_detection_apps_utils::displayHeads (heads, vis);
+ face_detection_apps_utils::displayHeads(heads, vis);
- vis.setRepresentationToSurfaceForAllActors ();
- vis.spinOnce ();
+ vis.setRepresentationToSurfaceForAllActors();
+ vis.spinOnce();
- vis.removeAllPointClouds ();
- vis.removeAllShapes ();
+ vis.removeAllPointClouds();
+ vis.removeAllShapes();
}
}
-//./bin/pcl_openni_face_detector -face_threshold 0.99 -max_variance 2400 -min_votes_size 300 -stride_sw 4 -heat_map 0 -show_votes 1 -pose_refinement 1 -icp_iterations 5 -model_path face_model.pcd -forest_fn forest.txt
-int main(int argc, char ** argv)
+int
+main(int argc, char** argv)
{
int STRIDE_SW = 4;
int use_normals = 0;
std::string forest_fn = "../data/forests/forest.txt";
std::string model_path_ = "../data/ply_models/face.pcd";
- pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn);
- pcl::console::parse_argument (argc, argv, "-max_variance", trans_max_variance);
- pcl::console::parse_argument (argc, argv, "-min_votes_size", min_votes_size);
- pcl::console::parse_argument (argc, argv, "-use_normals", use_normals);
- pcl::console::parse_argument (argc, argv, "-face_threshold", face_threshold);
- pcl::console::parse_argument (argc, argv, "-stride_sw", STRIDE_SW);
- pcl::console::parse_argument (argc, argv, "-heat_map", heat_map);
- pcl::console::parse_argument (argc, argv, "-show_votes", show_votes);
- pcl::console::parse_argument (argc, argv, "-pose_refinement", pose_refinement_);
- pcl::console::parse_argument (argc, argv, "-model_path", model_path_);
- pcl::console::parse_argument (argc, argv, "-icp_iterations", icp_iterations);
+ pcl::console::parse_argument(argc, argv, "-forest_fn", forest_fn);
+ pcl::console::parse_argument(argc, argv, "-max_variance", trans_max_variance);
+ pcl::console::parse_argument(argc, argv, "-min_votes_size", min_votes_size);
+ pcl::console::parse_argument(argc, argv, "-use_normals", use_normals);
+ pcl::console::parse_argument(argc, argv, "-face_threshold", face_threshold);
+ pcl::console::parse_argument(argc, argv, "-stride_sw", STRIDE_SW);
+ pcl::console::parse_argument(argc, argv, "-heat_map", heat_map);
+ pcl::console::parse_argument(argc, argv, "-show_votes", show_votes);
+ pcl::console::parse_argument(argc, argv, "-pose_refinement", pose_refinement_);
+ pcl::console::parse_argument(argc, argv, "-model_path", model_path_);
+ pcl::console::parse_argument(argc, argv, "-icp_iterations", icp_iterations);
pcl::RFFaceDetectorTrainer fdrf;
- fdrf.setForestFilename (forest_fn);
- fdrf.setWSize (80);
- fdrf.setUseNormals (static_cast<bool> (use_normals));
- fdrf.setWStride (STRIDE_SW);
- fdrf.setLeavesFaceMaxVariance (trans_max_variance);
- fdrf.setLeavesFaceThreshold (face_threshold);
- fdrf.setFaceMinVotes (min_votes_size);
-
- if (pose_refinement_)
- {
- fdrf.setPoseRefinement (true, icp_iterations);
- fdrf.setModelPath (model_path_);
+ fdrf.setForestFilename(forest_fn);
+ fdrf.setWSize(80);
+ fdrf.setUseNormals(static_cast<bool>(use_normals));
+ fdrf.setWStride(STRIDE_SW);
+ fdrf.setLeavesFaceMaxVariance(trans_max_variance);
+ fdrf.setLeavesFaceThreshold(face_threshold);
+ fdrf.setFaceMinVotes(min_votes_size);
+
+ if (pose_refinement_) {
+ fdrf.setPoseRefinement(true, icp_iterations);
+ fdrf.setModelPath(model_path_);
}
-//load forest from file and pass it to the detector
+ // load forest from file and pass it to the detector
std::filebuf fb;
- fb.open (forest_fn.c_str (), std::ios::in);
- std::istream os (&fb);
+ fb.open(forest_fn.c_str(), std::ios::in);
+ std::istream os(&fb);
using NodeType = pcl::face_detection::RFTreeNode<pcl::face_detection::FeatureType>;
pcl::DecisionForest<NodeType> forest;
- forest.deserialize (os);
- fb.close ();
+ forest.deserialize(os);
+ fb.close();
- fdrf.setForest (forest);
+ fdrf.setForest(forest);
- run (fdrf, heat_map, show_votes);
+ run(fdrf, heat_map, show_votes);
}
-
-#include "pcl/apps/face_detection/openni_frame_source.h"
+#include <pcl/apps/face_detection/openni_frame_source.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
-namespace OpenNIFrameSource
-{
+namespace OpenNIFrameSource {
- OpenNIFrameSource::OpenNIFrameSource(const std::string& device_id) :
- grabber_ (device_id), frame_counter_ (0), active_ (true)
- {
- std::function<void(const PointCloudConstPtr&)> frame_cb = [this] (const PointCloudConstPtr& cloud) { onNewFrame (cloud); };
- grabber_.registerCallback (frame_cb);
- grabber_.start ();
- }
+OpenNIFrameSource::OpenNIFrameSource(const std::string& device_id)
+: grabber_(device_id), frame_counter_(0), active_(true)
+{
+ std::function<void(const PointCloudConstPtr&)> frame_cb =
+ [this](const PointCloudConstPtr& cloud) { onNewFrame(cloud); };
+ grabber_.registerCallback(frame_cb);
+ grabber_.start();
+}
- OpenNIFrameSource::~OpenNIFrameSource()
- {
- // Stop the grabber when shutting down
- grabber_.stop ();
- }
+OpenNIFrameSource::~OpenNIFrameSource()
+{
+ // Stop the grabber when shutting down
+ grabber_.stop();
+}
- bool OpenNIFrameSource::isActive()
- {
- return active_;
- }
+bool
+OpenNIFrameSource::isActive() const
+{
+ return active_;
+}
- const PointCloudPtr OpenNIFrameSource::snap()
- {
- return (most_recent_frame_);
- }
+const PointCloudPtr
+OpenNIFrameSource::snap()
+{
+ return most_recent_frame_;
+}
- void OpenNIFrameSource::onNewFrame(const PointCloudConstPtr &cloud)
- {
- mutex_.lock ();
- ++frame_counter_;
- most_recent_frame_ = pcl::make_shared<PointCloud> (*cloud); // Make a copy of the frame
- mutex_.unlock ();
- }
+void
+OpenNIFrameSource::onNewFrame(const PointCloudConstPtr& cloud)
+{
+ mutex_.lock();
+ ++frame_counter_;
+ most_recent_frame_ = pcl::make_shared<PointCloud>(*cloud); // Make a copy of the frame
+ mutex_.unlock();
+}
- void OpenNIFrameSource::onKeyboardEvent(const pcl::visualization::KeyboardEvent & event)
- {
- // When the spacebar is pressed, trigger a frame capture
- mutex_.lock ();
- if (event.keyDown () && event.getKeySym () == "e")
- {
- active_ = false;
- }
- mutex_.unlock ();
+void
+OpenNIFrameSource::onKeyboardEvent(const pcl::visualization::KeyboardEvent& event)
+{
+ // When the spacebar is pressed, trigger a frame capture
+ mutex_.lock();
+ if (event.keyDown() && event.getKeySym() == "e") {
+ active_ = false;
}
-
+ mutex_.unlock();
}
+
+} // namespace OpenNIFrameSource
-#include <vector>
-#include <string>
-#include <sstream>
+#include <pcl/common/transforms.h>
+#include <pcl/features/3dsc.h>
+#include <pcl/features/fpfh_omp.h>
+#include <pcl/features/pfh.h>
+#include <pcl/features/pfhrgb.h>
+#include <pcl/features/shot_omp.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/registration/transforms.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/keypoints/harris_3d.h>
-#include <pcl/ModelCoefficients.h>
+#include <pcl/keypoints/sift_keypoint.h>
+#include <pcl/registration/correspondence_rejection_sample_consensus.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/transformation_estimation_svd.h>
+#include <pcl/registration/transforms.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
-#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/features/fpfh_omp.h>
-#include <pcl/features/pfh.h>
-#include <pcl/features/pfhrgb.h>
-#include <pcl/features/3dsc.h>
-#include <pcl/features/shot_omp.h>
-#include <pcl/registration/transformation_estimation_svd.h>
-#include <pcl/registration/icp.h>
-#include <pcl/registration/correspondence_rejection_sample_consensus.h>
-#include <pcl/common/transforms.h>
-#include <pcl/surface/grid_projection.h>
+#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/surface/gp3.h>
+#include <pcl/surface/grid_projection.h>
#include <pcl/surface/marching_cubes_hoppe.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/memory.h> // for pcl::dynamic_pointer_cast
-template<typename FeatureType>
-class ICCVTutorial
-{
- public:
- ICCVTutorial (pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector,
- typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor,
- pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor,
- pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
- pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target);
-
- /**
- * @brief starts the event loop for the visualizer
- */
- void run ();
- protected:
- /**
- * @brief remove plane and select largest cluster as input object
- * @param input the input point cloud
- * @param segmented the resulting segmented point cloud containing only points of the largest cluster
- */
- void segmentation (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input, const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr& segmented) const;
-
- /**
- * @brief Detects key points in the input point cloud
- * @param input the input point cloud
- * @param keypoints the resulting key points. Note that they are not necessarily a subset of the input cloud
- */
- void detectKeypoints (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input, const pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints) const;
-
- /**
- * @brief extract descriptors for given key points
- * @param input point cloud to be used for descriptor extraction
- * @param keypoints locations where descriptors are to be extracted
- * @param features resulting descriptors
- */
- void extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, const typename pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints, typename pcl::PointCloud<FeatureType>::Ptr features);
-
- /**
- * @brief find corresponding features based on some metric
- * @param source source feature descriptors
- * @param target target feature descriptors
- * @param correspondences indices out of the target descriptors that correspond (nearest neighbor) to the source descriptors
- */
- void findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const;
-
- /**
- * @brief remove non-consistent correspondences
- */
- void filterCorrespondences ();
-
- /**
- * @brief calculate the initial rigid transformation from filtered corresponding keypoints
- */
- void determineInitialTransformation ();
-
- /**
- * @brief calculate the final rigid transformation using ICP over all points
- */
- void determineFinalTransformation ();
-
- /**
- * @brief reconstructs the surface from merged point clouds
- */
- void reconstructSurface ();
-
- /**
- * @brief callback to handle keyboard events
- * @param event object containing information about the event. e.g. type (press, release) etc.
- * @param cookie user defined data passed during registration of the callback
- */
- void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* cookie);
-
- private:
- pcl::visualization::PCLVisualizer visualizer_;
- pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_;
- pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_;
- pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector_;
- typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor_;
- pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor_;
- typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source_;
- typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target_;
- typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_segmented_;
- typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_segmented_;
- typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_transformed_;
- typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registered_;
- typename pcl::PolygonMesh surface_;
- typename pcl::PointCloud<FeatureType>::Ptr source_features_;
- typename pcl::PointCloud<FeatureType>::Ptr target_features_;
- std::vector<int> source2target_;
- std::vector<int> target2source_;
- pcl::CorrespondencesPtr correspondences_;
- Eigen::Matrix4f initial_transformation_matrix_;
- Eigen::Matrix4f transformation_matrix_;
- bool show_source2target_;
- bool show_target2source_;
- bool show_correspondences;
+#include <sstream>
+#include <string>
+#include <vector>
+
+template <typename FeatureType>
+class ICCVTutorial {
+public:
+ ICCVTutorial(
+ pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector,
+ typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor,
+ pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor,
+ pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
+ pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target);
+
+ /**
+ * \brief starts the event loop for the visualizer
+ */
+ void
+ run();
+
+protected:
+ /**
+ * \brief remove plane and select largest cluster as input object
+ * \param input the input point cloud
+ * \param segmented the resulting segmented point cloud containing only points of the
+ * largest cluster
+ */
+ void
+ segmentation(const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input,
+ const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr& segmented) const;
+
+ /**
+ * \brief Detects key points in the input point cloud
+ * \param input the input point cloud
+ * \param keypoints the resulting key points. Note that they are not necessarily a
+ * subset of the input cloud
+ */
+ void
+ detectKeypoints(const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input,
+ const pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints) const;
+
+ /**
+ * \brief extract descriptors for given key points
+ * \param input point cloud to be used for descriptor extraction
+ * \param keypoints locations where descriptors are to be extracted
+ * \param features resulting descriptors
+ */
+ void
+ extractDescriptors(typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input,
+ const typename pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints,
+ typename pcl::PointCloud<FeatureType>::Ptr features);
+
+ /**
+ * \brief find corresponding features based on some metric
+ * \param source source feature descriptors
+ * \param target target feature descriptors
+ * \param correspondences indices out of the target descriptors that correspond
+ * (nearest neighbor) to the source descriptors
+ */
+ void
+ findCorrespondences(typename pcl::PointCloud<FeatureType>::Ptr source,
+ typename pcl::PointCloud<FeatureType>::Ptr target,
+ std::vector<int>& correspondences) const;
+
+ /**
+ * \brief remove non-consistent correspondences
+ */
+ void
+ filterCorrespondences();
+
+ /**
+ * \brief calculate the initial rigid transformation from filtered corresponding
+ * keypoints
+ */
+ void
+ determineInitialTransformation();
+
+ /**
+ * \brief calculate the final rigid transformation using ICP over all points
+ */
+ void
+ determineFinalTransformation();
+
+ /**
+ * \brief reconstructs the surface from merged point clouds
+ */
+ void
+ reconstructSurface();
+
+ /**
+ * \brief callback to handle keyboard events
+ * \param event object containing information about the event. e.g. type (press,
+ * release) etc.
+ * \param cookie user defined data passed during registration of the callback
+ */
+ void
+ keyboard_callback(const pcl::visualization::KeyboardEvent& event, void* cookie);
+
+private:
+ pcl::visualization::PCLVisualizer visualizer_;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_;
+ pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector_;
+ typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor_;
+ pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor_;
+ typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source_;
+ typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target_;
+ typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_segmented_;
+ typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_segmented_;
+ typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_transformed_;
+ typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registered_;
+ typename pcl::PolygonMesh surface_;
+ typename pcl::PointCloud<FeatureType>::Ptr source_features_;
+ typename pcl::PointCloud<FeatureType>::Ptr target_features_;
+ std::vector<int> source2target_;
+ std::vector<int> target2source_;
+ pcl::CorrespondencesPtr correspondences_;
+ Eigen::Matrix4f initial_transformation_matrix_;
+ Eigen::Matrix4f transformation_matrix_;
+ bool show_source2target_;
+ bool show_target2source_;
+ bool show_correspondences;
};
-template<typename FeatureType>
-ICCVTutorial<FeatureType>::ICCVTutorial(pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector,
- typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor,
- pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor,
- pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
- pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target)
-: source_keypoints_ (new pcl::PointCloud<pcl::PointXYZI> ())
-, target_keypoints_ (new pcl::PointCloud<pcl::PointXYZI> ())
-, keypoint_detector_ (std::move(keypoint_detector))
-, feature_extractor_ (feature_extractor)
-, surface_reconstructor_ (std::move(surface_reconstructor))
-, source_ (std::move(source))
-, target_ (std::move(target))
-, source_segmented_ (new pcl::PointCloud<pcl::PointXYZRGB>)
-, target_segmented_ (new pcl::PointCloud<pcl::PointXYZRGB>)
-, source_transformed_ (new pcl::PointCloud<pcl::PointXYZRGB>)
-, source_registered_ (new pcl::PointCloud<pcl::PointXYZRGB>)
-, source_features_ (new pcl::PointCloud<FeatureType>)
-, target_features_ (new pcl::PointCloud<FeatureType>)
-, correspondences_ (new pcl::Correspondences)
-, show_source2target_ (false)
-, show_target2source_ (false)
-, show_correspondences (false)
+template <typename FeatureType>
+ICCVTutorial<FeatureType>::ICCVTutorial(
+ pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector,
+ typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor,
+ pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor,
+ pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
+ pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target)
+: source_keypoints_(new pcl::PointCloud<pcl::PointXYZI>())
+, target_keypoints_(new pcl::PointCloud<pcl::PointXYZI>())
+, keypoint_detector_(std::move(keypoint_detector))
+, feature_extractor_(feature_extractor)
+, surface_reconstructor_(std::move(surface_reconstructor))
+, source_(std::move(source))
+, target_(std::move(target))
+, source_segmented_(new pcl::PointCloud<pcl::PointXYZRGB>)
+, target_segmented_(new pcl::PointCloud<pcl::PointXYZRGB>)
+, source_transformed_(new pcl::PointCloud<pcl::PointXYZRGB>)
+, source_registered_(new pcl::PointCloud<pcl::PointXYZRGB>)
+, source_features_(new pcl::PointCloud<FeatureType>)
+, target_features_(new pcl::PointCloud<FeatureType>)
+, correspondences_(new pcl::Correspondences)
+, show_source2target_(false)
+, show_target2source_(false)
+, show_correspondences(false)
{
- visualizer_.registerKeyboardCallback(&ICCVTutorial::keyboard_callback, *this, nullptr);
+ visualizer_.registerKeyboardCallback(
+ &ICCVTutorial::keyboard_callback, *this, nullptr);
- segmentation (source_, source_segmented_);
- segmentation (target_, target_segmented_);
+ segmentation(source_, source_segmented_);
+ segmentation(target_, target_segmented_);
- detectKeypoints (source_segmented_, source_keypoints_);
- detectKeypoints (target_segmented_, target_keypoints_);
+ detectKeypoints(source_segmented_, source_keypoints_);
+ detectKeypoints(target_segmented_, target_keypoints_);
- extractDescriptors (source_segmented_, source_keypoints_, source_features_);
- extractDescriptors (target_segmented_, target_keypoints_, target_features_);
+ extractDescriptors(source_segmented_, source_keypoints_, source_features_);
+ extractDescriptors(target_segmented_, target_keypoints_, target_features_);
- findCorrespondences (source_features_, target_features_, source2target_);
- findCorrespondences (target_features_, source_features_, target2source_);
+ findCorrespondences(source_features_, target_features_, source2target_);
+ findCorrespondences(target_features_, source_features_, target2source_);
- filterCorrespondences ();
+ filterCorrespondences();
- determineInitialTransformation ();
- determineFinalTransformation ();
+ determineInitialTransformation();
+ determineFinalTransformation();
- reconstructSurface ();
+ reconstructSurface();
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::segmentation (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& source, const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr& segmented) const
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::segmentation(
+ const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& source,
+ const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr& segmented) const
{
std::cout << "segmentation..." << std::flush;
// fit plane and keep points above that plane
- pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
- pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+ pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
- seg.setOptimizeCoefficients (true);
+ seg.setOptimizeCoefficients(true);
// Mandatory
- seg.setModelType (pcl::SACMODEL_PLANE);
- seg.setMethodType (pcl::SAC_RANSAC);
- seg.setDistanceThreshold (0.02);
+ seg.setModelType(pcl::SACMODEL_PLANE);
+ seg.setMethodType(pcl::SAC_RANSAC);
+ seg.setDistanceThreshold(0.02);
- seg.setInputCloud (source);
- seg.segment (*inliers, *coefficients);
+ seg.setInputCloud(source);
+ seg.segment(*inliers, *coefficients);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
- extract.setInputCloud (source);
- extract.setIndices (inliers);
- extract.setNegative (true);
+ extract.setInputCloud(source);
+ extract.setIndices(inliers);
+ extract.setNegative(true);
- extract.filter (*segmented);
+ extract.filter(*segmented);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*segmented, *segmented, indices);
std::cout << "OK" << std::endl;
std::cout << "clustering..." << std::flush;
// euclidean clustering
- typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
- tree->setInputCloud (segmented);
+ typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(
+ new pcl::search::KdTree<pcl::PointXYZRGB>);
+ tree->setInputCloud(segmented);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering;
- clustering.setClusterTolerance (0.02); // 2cm
- clustering.setMinClusterSize (1000);
- clustering.setMaxClusterSize (250000);
- clustering.setSearchMethod (tree);
+ clustering.setClusterTolerance(0.02); // 2cm
+ clustering.setMinClusterSize(1000);
+ clustering.setMaxClusterSize(250000);
+ clustering.setSearchMethod(tree);
clustering.setInputCloud(segmented);
- clustering.extract (cluster_indices);
+ clustering.extract(cluster_indices);
- if (!cluster_indices.empty ())//use largest cluster
+ if (!cluster_indices.empty()) // use largest cluster
{
std::cout << cluster_indices.size() << " clusters found";
if (cluster_indices.size() > 1)
- std::cout <<" Using largest one...";
+ std::cout << " Using largest one...";
std::cout << std::endl;
- typename pcl::IndicesPtr indices (new std::vector<int>);
+ typename pcl::IndicesPtr indices(new std::vector<int>);
*indices = cluster_indices[0].indices;
- extract.setInputCloud (segmented);
- extract.setIndices (indices);
- extract.setNegative (false);
+ extract.setInputCloud(segmented);
+ extract.setIndices(indices);
+ extract.setNegative(false);
- extract.filter (*segmented);
+ extract.filter(*segmented);
}
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::detectKeypoints (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input, const pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints) const
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::detectKeypoints(
+ const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input,
+ const pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints) const
{
std::cout << "keypoint detection..." << std::flush;
keypoint_detector_->setInputCloud(input);
std::cout << "OK. keypoints found: " << keypoints->points.size() << std::endl;
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, const typename pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints, typename pcl::PointCloud<FeatureType>::Ptr features)
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::extractDescriptors(
+ typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input,
+ const typename pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints,
+ typename pcl::PointCloud<FeatureType>::Ptr features)
{
- typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(new pcl::PointCloud<pcl::PointXYZRGB>);
+ typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(
+ new pcl::PointCloud<pcl::PointXYZRGB>);
kpts->points.resize(keypoints->points.size());
pcl::copyPointCloud(*keypoints, *kpts);
- typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType>::Ptr feature_from_normals = boost::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_);
+ typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType>::Ptr
+ feature_from_normals = pcl::dynamic_pointer_cast<
+ pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType>>(
+ feature_extractor_);
feature_extractor_->setSearchSurface(input);
feature_extractor_->setInputCloud(kpts);
- if (feature_from_normals)
- //if (boost::dynamic_pointer_cast<typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_))
- {
+ if (feature_from_normals) {
std::cout << "normal estimation..." << std::flush;
- typename pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+ typename pcl::PointCloud<pcl::Normal>::Ptr normals(
+ new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimation;
- normal_estimation.setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
- normal_estimation.setRadiusSearch (0.01);
- normal_estimation.setInputCloud (input);
- normal_estimation.compute (*normals);
+ normal_estimation.setSearchMethod(pcl::search::Search<pcl::PointXYZRGB>::Ptr(
+ new pcl::search::KdTree<pcl::PointXYZRGB>));
+ normal_estimation.setRadiusSearch(0.01);
+ normal_estimation.setInputCloud(input);
+ normal_estimation.compute(*normals);
feature_from_normals->setInputNormals(normals);
std::cout << "OK" << std::endl;
}
std::cout << "descriptor extraction..." << std::flush;
- feature_extractor_->compute (*features);
+ feature_extractor_->compute(*features);
std::cout << "OK" << std::endl;
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::findCorrespondences(
+ typename pcl::PointCloud<FeatureType>::Ptr source,
+ typename pcl::PointCloud<FeatureType>::Ptr target,
+ std::vector<int>& correspondences) const
{
std::cout << "correspondence assignment..." << std::flush;
- correspondences.resize (source->size());
+ correspondences.resize(source->size());
// Use a KdTree to search for the nearest matches in feature space
pcl::KdTreeFLANN<FeatureType> descriptor_kdtree;
- descriptor_kdtree.setInputCloud (target);
+ descriptor_kdtree.setInputCloud(target);
- // Find the index of the best match for each keypoint, and store it in "correspondences_out"
+ // Find the index of the best match for each keypoint, and store it in
+ // "correspondences_out"
const int k = 1;
- std::vector<int> k_indices (k);
- std::vector<float> k_squared_distances (k);
- for (int i = 0; i < static_cast<int> (source->size ()); ++i)
- {
- descriptor_kdtree.nearestKSearch (*source, i, k, k_indices, k_squared_distances);
+ std::vector<int> k_indices(k);
+ std::vector<float> k_squared_distances(k);
+ for (int i = 0; i < static_cast<int>(source->size()); ++i) {
+ descriptor_kdtree.nearestKSearch(*source, i, k, k_indices, k_squared_distances);
correspondences[i] = k_indices[0];
}
std::cout << "OK" << std::endl;
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::filterCorrespondences ()
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::filterCorrespondences()
{
std::cout << "correspondence rejection..." << std::flush;
- std::vector<std::pair<unsigned, unsigned> > correspondences;
- for (std::size_t cIdx = 0; cIdx < source2target_.size (); ++cIdx)
- if (target2source_[source2target_[cIdx]] == static_cast<int> (cIdx))
+ std::vector<std::pair<unsigned, unsigned>> correspondences;
+ for (std::size_t cIdx = 0; cIdx < source2target_.size(); ++cIdx)
+ if (target2source_[source2target_[cIdx]] == static_cast<int>(cIdx))
correspondences.push_back(std::make_pair(cIdx, source2target_[cIdx]));
- correspondences_->resize (correspondences.size());
- for (std::size_t cIdx = 0; cIdx < correspondences.size(); ++cIdx)
- {
+ correspondences_->resize(correspondences.size());
+ for (std::size_t cIdx = 0; cIdx < correspondences.size(); ++cIdx) {
(*correspondences_)[cIdx].index_query = correspondences[cIdx].first;
(*correspondences_)[cIdx].index_match = correspondences[cIdx].second;
}
pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZI> rejector;
- rejector.setInputSource (source_keypoints_);
- rejector.setInputTarget (target_keypoints_);
+ rejector.setInputSource(source_keypoints_);
+ rejector.setInputTarget(target_keypoints_);
rejector.setInputCorrespondences(correspondences_);
rejector.getCorrespondences(*correspondences_);
std::cout << "OK" << std::endl;
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::determineInitialTransformation ()
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::determineInitialTransformation()
{
std::cout << "initial alignment..." << std::flush;
- pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>::Ptr transformation_estimation (new pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI>);
-
- transformation_estimation->estimateRigidTransformation (*source_keypoints_, *target_keypoints_, *correspondences_, initial_transformation_matrix_);
-
- pcl::transformPointCloud(*source_segmented_, *source_transformed_, initial_transformation_matrix_);
+ pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>::Ptr
+ transformation_estimation(
+ new pcl::registration::TransformationEstimationSVD<pcl::PointXYZI,
+ pcl::PointXYZI>);
+
+ transformation_estimation->estimateRigidTransformation(
+ *source_keypoints_,
+ *target_keypoints_,
+ *correspondences_,
+ initial_transformation_matrix_);
+
+ pcl::transformPointCloud(
+ *source_segmented_, *source_transformed_, initial_transformation_matrix_);
std::cout << "OK" << std::endl;
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::determineFinalTransformation ()
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::determineFinalTransformation()
{
std::cout << "final registration..." << std::flush;
- pcl::Registration<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr registration (new pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB>);
- registration->setInputSource (source_transformed_);
- //registration->setInputSource (source_segmented_);
- registration->setInputTarget (target_segmented_);
+ pcl::Registration<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr registration(
+ new pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB>);
+ registration->setInputSource(source_transformed_);
+ // registration->setInputSource (source_segmented_);
+ registration->setInputTarget(target_segmented_);
registration->setMaxCorrespondenceDistance(0.05);
- registration->setRANSACOutlierRejectionThreshold (0.05);
- registration->setTransformationEpsilon (0.000001);
- registration->setMaximumIterations (1000);
+ registration->setRANSACOutlierRejectionThreshold(0.05);
+ registration->setTransformationEpsilon(0.000001);
+ registration->setMaximumIterations(1000);
registration->align(*source_registered_);
transformation_matrix_ = registration->getFinalTransformation();
std::cout << "OK" << std::endl;
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::reconstructSurface ()
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::reconstructSurface()
{
std::cout << "surface reconstruction..." << std::flush;
// merge the transformed and the target point cloud
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr merged
(new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr merged(new pcl::PointCloud<pcl::PointXYZRGB>);
*merged = *source_registered_;
*merged += *target_segmented_;
- // apply grid filtering to reduce amount of points as well as to make them uniform distributed
+ // apply grid filtering to reduce amount of points as well as to make them uniform
+ // distributed
pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
voxel_grid.setInputCloud(merged);
- voxel_grid.setLeafSize (0.002f, 0.002f, 0.002f);
+ voxel_grid.setLeafSize(0.002f, 0.002f, 0.002f);
voxel_grid.setDownsampleAllData(true);
voxel_grid.filter(*merged);
- pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr vertices (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr vertices(
+ new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::copyPointCloud(*merged, *vertices);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> normal_estimation;
- normal_estimation.setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
- normal_estimation.setRadiusSearch (0.01);
- normal_estimation.setInputCloud (merged);
- normal_estimation.compute (*vertices);
+ normal_estimation.setSearchMethod(pcl::search::Search<pcl::PointXYZRGB>::Ptr(
+ new pcl::search::KdTree<pcl::PointXYZRGB>));
+ normal_estimation.setRadiusSearch(0.01);
+ normal_estimation.setInputCloud(merged);
+ normal_estimation.compute(*vertices);
- pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
- tree->setInputCloud (vertices);
+ pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(
+ new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
+ tree->setInputCloud(vertices);
surface_reconstructor_->setSearchMethod(tree);
surface_reconstructor_->setInputCloud(vertices);
std::cout << "OK" << std::endl;
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::run()
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::run()
{
- visualizer_.spin ();
+ visualizer_.spin();
}
-template<typename FeatureType>
-void ICCVTutorial<FeatureType>::keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
+template <typename FeatureType>
+void
+ICCVTutorial<FeatureType>::keyboard_callback(
+ const pcl::visualization::KeyboardEvent& event, void*)
{
- if (event.keyUp())
- {
- switch (event.getKeyCode())
- {
- case '1':
- if (!visualizer_.removePointCloud("source_points"))
- {
- visualizer_.addPointCloud(source_, "source_points");
- }
- break;
-
- case '2':
- if (!visualizer_.removePointCloud("target_points"))
- {
- visualizer_.addPointCloud(target_, "target_points");
- }
- break;
-
- case '3':
- if (!visualizer_.removePointCloud("source_segmented"))
- {
- visualizer_.addPointCloud(source_segmented_, "source_segmented");
- }
- break;
-
- case '4':
- if (!visualizer_.removePointCloud("target_segmented"))
- {
- visualizer_.addPointCloud(target_segmented_, "target_segmented");
- }
- break;
-
- case '5':
- if (!visualizer_.removePointCloud("source_keypoints"))
- {
- pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color (source_keypoints_, 0, 0, 255);
- //pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> keypoint_color (source_keypoints_, "intensity");
- visualizer_.addPointCloud(source_keypoints_, keypoint_color, "source_keypoints");
- }
- break;
-
- case '6':
- if (!visualizer_.removePointCloud("target_keypoints"))
- {
- //pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> keypoint_color (target_keypoints_, "intensity");
- pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color (target_keypoints_, 255, 0, 0);
- visualizer_.addPointCloud(target_keypoints_, keypoint_color, "target_keypoints");
- }
- break;
-
- case '7':
- if (!show_source2target_)
- visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, source2target_, "source2target");
- else
- visualizer_.removeCorrespondences("source2target");
-
- show_source2target_ = !show_source2target_;
- break;
-
- case '8':
- if (!show_target2source_)
- visualizer_.addCorrespondences<pcl::PointXYZI>(target_keypoints_, source_keypoints_, target2source_, "target2source");
- else
- visualizer_.removeCorrespondences("target2source");
-
- show_target2source_ = !show_target2source_;
- break;
-
- case '9':
- if (!show_correspondences)
- visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, *correspondences_, "correspondences");
- else
- visualizer_.removeCorrespondences("correspondences");
- show_correspondences = !show_correspondences;
- break;
-
- case 'i':
- case 'I':
- if (!visualizer_.removePointCloud("transformed"))
- visualizer_.addPointCloud(source_transformed_, "transformed");
- break;
-
- case 'r':
- case 'R':
- if (!visualizer_.removePointCloud("registered"))
- visualizer_.addPointCloud(source_registered_, "registered");
- break;
-
- case 't':
- case 'T':
- visualizer_.addPolygonMesh(surface_, "surface");
- break;
+ if (event.keyUp()) {
+ switch (event.getKeyCode()) {
+ case '1':
+ if (!visualizer_.removePointCloud("source_points")) {
+ visualizer_.addPointCloud(source_, "source_points");
+ }
+ break;
+
+ case '2':
+ if (!visualizer_.removePointCloud("target_points")) {
+ visualizer_.addPointCloud(target_, "target_points");
+ }
+ break;
+
+ case '3':
+ if (!visualizer_.removePointCloud("source_segmented")) {
+ visualizer_.addPointCloud(source_segmented_, "source_segmented");
+ }
+ break;
+
+ case '4':
+ if (!visualizer_.removePointCloud("target_segmented")) {
+ visualizer_.addPointCloud(target_segmented_, "target_segmented");
+ }
+ break;
+
+ case '5':
+ if (!visualizer_.removePointCloud("source_keypoints")) {
+ pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color(
+ source_keypoints_, 0, 0, 255);
+ visualizer_.addPointCloud(
+ source_keypoints_, keypoint_color, "source_keypoints");
+ }
+ break;
+
+ case '6':
+ if (!visualizer_.removePointCloud("target_keypoints")) {
+ pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color(
+ target_keypoints_, 255, 0, 0);
+ visualizer_.addPointCloud(
+ target_keypoints_, keypoint_color, "target_keypoints");
+ }
+ break;
+
+ case '7':
+ if (!show_source2target_)
+ visualizer_.addCorrespondences<pcl::PointXYZI>(
+ source_keypoints_, target_keypoints_, source2target_, "source2target");
+ else
+ visualizer_.removeCorrespondences("source2target");
+
+ show_source2target_ = !show_source2target_;
+ break;
+
+ case '8':
+ if (!show_target2source_)
+ visualizer_.addCorrespondences<pcl::PointXYZI>(
+ target_keypoints_, source_keypoints_, target2source_, "target2source");
+ else
+ visualizer_.removeCorrespondences("target2source");
+
+ show_target2source_ = !show_target2source_;
+ break;
+
+ case '9':
+ if (!show_correspondences)
+ visualizer_.addCorrespondences<pcl::PointXYZI>(
+ source_keypoints_, target_keypoints_, *correspondences_, "correspondences");
+ else
+ visualizer_.removeCorrespondences("correspondences");
+ show_correspondences = !show_correspondences;
+ break;
+
+ case 'i':
+ case 'I':
+ if (!visualizer_.removePointCloud("transformed"))
+ visualizer_.addPointCloud(source_transformed_, "transformed");
+ break;
+
+ case 'r':
+ case 'R':
+ if (!visualizer_.removePointCloud("registered"))
+ visualizer_.addPointCloud(source_registered_, "registered");
+ break;
+
+ case 't':
+ case 'T':
+ visualizer_.addPolygonMesh(surface_, "surface");
+ break;
}
}
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
- if (argc < 6)
- {
+ if (argc < 6) {
+ // clang-format off
pcl::console::print_info ("Syntax is: %s <source-pcd-file> <target-pcd-file> <keypoint-method> <descriptor-type> <surface-reconstruction-method>\n", argv[0]);
- pcl::console::print_info ("available <keypoint-methods>: 1 = Sift3D\n");
- pcl::console::print_info (" 2 = Harris3D\n");
- pcl::console::print_info (" 3 = Tomasi3D\n");
- pcl::console::print_info (" 4 = Noble3D\n");
- pcl::console::print_info (" 5 = Lowe3D\n");
- pcl::console::print_info (" 6 = Curvature3D\n\n");
- pcl::console::print_info ("available <descriptor-types>: 1 = FPFH\n");
- pcl::console::print_info (" 2 = SHOTRGB\n");
- pcl::console::print_info (" 3 = PFH\n");
- pcl::console::print_info (" 4 = PFHRGB\n\n");
- pcl::console::print_info ("available <surface-methods>: 1 = Greedy Projection\n");
- pcl::console::print_info (" 2 = Marching Cubes\n");
-
- return (1);
+ pcl::console::print_info("available <keypoint-methods>: 1 = Sift3D\n");
+ pcl::console::print_info(" 2 = Harris3D\n");
+ pcl::console::print_info(" 3 = Tomasi3D\n");
+ pcl::console::print_info(" 4 = Noble3D\n");
+ pcl::console::print_info(" 5 = Lowe3D\n");
+ pcl::console::print_info(" 6 = Curvature3D\n\n");
+ pcl::console::print_info("available <descriptor-types>: 1 = FPFH\n");
+ pcl::console::print_info(" 2 = SHOTRGB\n");
+ pcl::console::print_info(" 3 = PFH\n");
+ pcl::console::print_info(" 4 = PFHRGB\n\n");
+ pcl::console::print_info("available <surface-methods>: 1 = Greedy Projection\n");
+ pcl::console::print_info(" 2 = Marching Cubes\n");
+ // clang-format on
+
+ return 1;
}
- pcl::console::print_info ("== MENU ==\n");
- pcl::console::print_info ("1 - show/hide source point cloud\n");
- pcl::console::print_info ("2 - show/hide target point cloud\n");
- pcl::console::print_info ("3 - show/hide segmented source point cloud\n");
- pcl::console::print_info ("4 - show/hide segmented target point cloud\n");
- pcl::console::print_info ("5 - show/hide source key points\n");
- pcl::console::print_info ("6 - show/hide target key points\n");
- pcl::console::print_info ("7 - show/hide source2target correspondences\n");
- pcl::console::print_info ("8 - show/hide target2source correspondences\n");
- pcl::console::print_info ("9 - show/hide consistent correspondences\n");
- pcl::console::print_info ("i - show/hide initial alignment\n");
- pcl::console::print_info ("r - show/hide final registration\n");
- pcl::console::print_info ("t - show/hide triangulation (surface reconstruction)\n");
- pcl::console::print_info ("h - show visualizer options\n");
- pcl::console::print_info ("q - quit\n");
-
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr source
(new pcl::PointCloud<pcl::PointXYZRGB>);
- pcl::io::loadPCDFile (argv[1], *source);
-
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr target
(new pcl::PointCloud<pcl::PointXYZRGB>);
- pcl::io::loadPCDFile (argv[2], *target);
-
- int keypoint_type = atoi (argv[3]);
- int descriptor_type = atoi (argv[4]);
- int surface_type = atoi (argv[5]);
+ pcl::console::print_info("== MENU ==\n");
+ pcl::console::print_info("1 - show/hide source point cloud\n");
+ pcl::console::print_info("2 - show/hide target point cloud\n");
+ pcl::console::print_info("3 - show/hide segmented source point cloud\n");
+ pcl::console::print_info("4 - show/hide segmented target point cloud\n");
+ pcl::console::print_info("5 - show/hide source key points\n");
+ pcl::console::print_info("6 - show/hide target key points\n");
+ pcl::console::print_info("7 - show/hide source2target correspondences\n");
+ pcl::console::print_info("8 - show/hide target2source correspondences\n");
+ pcl::console::print_info("9 - show/hide consistent correspondences\n");
+ pcl::console::print_info("i - show/hide initial alignment\n");
+ pcl::console::print_info("r - show/hide final registration\n");
+ pcl::console::print_info("t - show/hide triangulation (surface reconstruction)\n");
+ pcl::console::print_info("h - show visualizer options\n");
+ pcl::console::print_info("q - quit\n");
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr source(new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::io::loadPCDFile(argv[1], *source);
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr target(new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::io::loadPCDFile(argv[2], *target);
+
+ int keypoint_type = atoi(argv[3]);
+ int descriptor_type = atoi(argv[4]);
+ int surface_type = atoi(argv[5]);
pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector;
- if (keypoint_type == 1)
- {
-
pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>* sift3D = new pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>;
- sift3D->setScales (0.01f, 3, 2);
- sift3D->setMinimumContrast (0.0);
- keypoint_detector.reset (sift3D);
+ if (keypoint_type == 1) {
+ pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>* sift3D =
+
new pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>;
+ sift3D->setScales(0.01f, 3, 2);
+ sift3D->setMinimumContrast(0.0);
+ keypoint_detector.reset(sift3D);
}
- else
- {
- pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>* harris3D = new pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI> (pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::HARRIS);
- harris3D->setNonMaxSupression (true);
- harris3D->setRadius (0.01f);
- harris3D->setRadiusSearch (0.01f);
- keypoint_detector.reset (harris3D);
- switch (keypoint_type)
- {
- case 2:
- harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::HARRIS);
+ else {
+ pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>* harris3D =
+ new pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>(
+ pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::HARRIS);
+ harris3D->setNonMaxSupression(true);
+ harris3D->setRadius(0.01f);
+ harris3D->setRadiusSearch(0.01f);
+ keypoint_detector.reset(harris3D);
+ switch (keypoint_type) {
+ case 2:
+ harris3D->setMethod(
+ pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::HARRIS);
break;
- case 3:
- harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::TOMASI);
+ case 3:
+ harris3D->setMethod(
+ pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::TOMASI);
break;
- case 4:
- harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::NOBLE);
+ case 4:
+ harris3D->setMethod(
+ pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::NOBLE);
break;
- case 5:
- harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::LOWE);
+ case 5:
+ harris3D->setMethod(
+ pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::LOWE);
break;
- case 6:
- harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::CURVATURE);
+ case 6:
+ harris3D->setMethod(
+ pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::CURVATURE);
+ break;
+ default:
+ pcl::console::print_error(
+ "unknown key point detection method %d\n expecting values between 1 and 6",
+ keypoint_type);
+ exit(1);
break;
- default:
- pcl::console::print_error("unknown key point detection method %d\n expecting values between 1 and 6", keypoint_type);
- exit (1);
- break;
}
-
}
pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstruction;
- if (surface_type == 1)
- {
-
pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>* gp3 = new pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>;
+ if (surface_type == 1) {
+ pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>* gp3 =
+
new pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>;
// Set the maximum distance between connected points (maximum edge length)
- gp3->setSearchRadius (0.025);
+ gp3->setSearchRadius(0.025);
// Set typical values for the parameters
- gp3->setMu (2.5);
- gp3->setMaximumNearestNeighbors (100);
- gp3->setMaximumSurfaceAngle(M_PI/4); // 45 degrees
- gp3->setMinimumAngle(M_PI/18); // 10 degrees
- gp3->setMaximumAngle(2*M_PI/3); // 120 degrees
+ gp3->setMu(2.5);
+ gp3->setMaximumNearestNeighbors(100);
+ gp3->setMaximumSurfaceAngle(M_PI / 4); // 45 degrees
+ gp3->setMinimumAngle(M_PI / 18); // 10 degrees
+ gp3->setMaximumAngle(2 * M_PI / 3); // 120 degrees
gp3->setNormalConsistency(false);
surface_reconstruction.reset(gp3);
}
- else if (surface_type == 2)
- {
-
pcl::MarchingCubes<pcl::PointXYZRGBNormal>* mc = new pcl::MarchingCubesHoppe<pcl::PointXYZRGBNormal>;
- mc->setIsoLevel (0.001f);
- mc->setGridResolution (50, 50, 50);
+ else if (surface_type == 2) {
+ pcl::MarchingCubes<pcl::PointXYZRGBNormal>* mc =
+
new pcl::MarchingCubesHoppe<pcl::PointXYZRGBNormal>;
+ mc->setIsoLevel(0.001f);
+ mc->setGridResolution(50, 50, 50);
surface_reconstruction.reset(mc);
}
- else
- {
- pcl::console::print_error("unknown surface reconstruction method %d\n expecting values between 1 and 2", surface_type);
- exit (1);
+ else {
+ pcl::console::print_error(
+ "unknown surface reconstruction method %d\n expecting values between 1 and 2",
+ surface_type);
+ exit(1);
}
- switch (descriptor_type)
- {
- case 1:
- {
- pcl::Feature<pcl::PointXYZRGB, pcl::FPFHSignature33>::Ptr feature_extractor (new pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33>);
- feature_extractor->setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
- feature_extractor->setRadiusSearch (0.05);
- ICCVTutorial<pcl::FPFHSignature33> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
- tutorial.run ();
- }
- break;
-
- case 2:
- {
- pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>* shot = new pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>;
- shot->setRadiusSearch (0.04);
- pcl::Feature<pcl::PointXYZRGB, pcl::SHOT1344>::Ptr feature_extractor (shot);
- ICCVTutorial<pcl::SHOT1344> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
- tutorial.run ();
- }
- break;
-
- case 3:
- {
- pcl::Feature<pcl::PointXYZRGB, pcl::PFHSignature125>::Ptr feature_extractor (new pcl::PFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHSignature125>);
- feature_extractor->setKSearch(50);
- ICCVTutorial<pcl::PFHSignature125> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
- tutorial.run ();
- }
- break;
-
- case 4:
- {
- pcl::Feature<pcl::PointXYZRGB, pcl::PFHRGBSignature250>::Ptr feature_extractor (new pcl::PFHRGBEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHRGBSignature250>);
- feature_extractor->setKSearch(50);
- ICCVTutorial<pcl::PFHRGBSignature250> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
- tutorial.run ();
- }
+ switch (descriptor_type) {
+ case 1: {
+ pcl::Feature<pcl::PointXYZRGB, pcl::FPFHSignature33>::Ptr feature_extractor(
+ new pcl::
+ FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33>);
+ feature_extractor->setSearchMethod(pcl::search::Search<pcl::PointXYZRGB>::Ptr(
+ new pcl::search::KdTree<pcl::PointXYZRGB>));
+ feature_extractor->setRadiusSearch(0.05);
+ ICCVTutorial<pcl::FPFHSignature33> tutorial(
+ keypoint_detector, feature_extractor, surface_reconstruction, source, target);
+ tutorial.run();
+ } break;
+
+ case 2: {
+ pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>* shot =
+ new pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>;
+ shot->setRadiusSearch(0.04);
+ pcl::Feature<pcl::PointXYZRGB, pcl::SHOT1344>::Ptr feature_extractor(shot);
+ ICCVTutorial<pcl::SHOT1344> tutorial(
+ keypoint_detector, feature_extractor, surface_reconstruction, source, target);
+ tutorial.run();
+ } break;
+
+ case 3: {
+ pcl::Feature<pcl::PointXYZRGB, pcl::PFHSignature125>::Ptr feature_extractor(
+ new pcl::PFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHSignature125>);
+ feature_extractor->setKSearch(50);
+ ICCVTutorial<pcl::PFHSignature125> tutorial(
+ keypoint_detector, feature_extractor, surface_reconstruction, source, target);
+ tutorial.run();
+ } break;
+
+ case 4: {
+ pcl::Feature<pcl::PointXYZRGB, pcl::PFHRGBSignature250>::Ptr feature_extractor(
+ new pcl::
+ PFHRGBEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHRGBSignature250>);
+ feature_extractor->setKSearch(50);
+ ICCVTutorial<pcl::PFHRGBSignature250> tutorial(
+ keypoint_detector, feature_extractor, surface_reconstruction, source, target);
+ tutorial.run();
+ } break;
+
+ default:
+ pcl::console::print_error(
+ "unknown descriptor type %d\n expecting values between 1 and 4",
+ descriptor_type);
+ exit(1);
break;
-
- default:
- pcl::console::print_error("unknown descriptor type %d\n expecting values between 1 and 4", descriptor_type);
- exit (1);
- break;
}
- return (0);
+ return 0;
}
-#include <pcl/point_cloud.h>
-#include <pcl/segmentation/grabcut_segmentation.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/console/print.h>
#include <pcl/console/parse.h>
-#include <pcl/point_types.h>
+#include <pcl/console/print.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/segmentation/grabcut_segmentation.h>
#include <pcl/PCLPointCloud2.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#ifdef GLUT_IS_A_FRAMEWORK
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
-#if defined (FREEGLUT)
+#if defined(FREEGLUT)
#include <GL/freeglut_ext.h>
-#elif defined (GLUI_OPENGLUT)
+#elif defined(GLUI_OPENGLUT)
#include <GL/openglut.h>
#endif
#endif
-class GrabCutHelper : public pcl::GrabCut<pcl::PointXYZRGB>
-{
+class GrabCutHelper : public pcl::GrabCut<pcl::PointXYZRGB> {
using pcl::GrabCut<pcl::PointXYZRGB>::n_links_;
using pcl::GrabCut<pcl::PointXYZRGB>::graph_;
using pcl::GrabCut<pcl::PointXYZRGB>::indices_;
using pcl::GrabCut<pcl::PointXYZRGB>::GMM_component_;
using pcl::GrabCut<pcl::PointXYZRGB>::input_;
- public:
+public:
using Ptr = std::shared_ptr<GrabCutHelper>;
using ConstPtr = std::shared_ptr<const GrabCutHelper>;
- GrabCutHelper (std::uint32_t K = 5, float lambda = 50.f)
- : pcl::GrabCut<pcl::PointXYZRGB> (K, lambda)
+ GrabCutHelper(std::uint32_t K = 5, float lambda = 50.f)
+ : pcl::GrabCut<pcl::PointXYZRGB>(K, lambda)
{}
- ~GrabCutHelper ()
- { }
+ ~GrabCutHelper() {}
void
- setInputCloud
(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud) override;
+ setInputCloud(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud) override;
void
- setBackgroundPointsIndices (const pcl::PointIndices::ConstPtr& point_indices);
+ setBackgroundPointsIndices(const pcl::PointIndices::ConstPtr& point_indices);
void
- setBackgroundPointsIndices (int x1, int y1, int x2, int y2);
+ setBackgroundPointsIndices(int x1, int y1, int x2, int y2);
void
- setTrimap(int x1, int y1, int x2, int y2, const pcl::segmentation::grabcut::TrimapValue& t);
+ setTrimap(
+ int x1, int y1, int x2, int y2, const pcl::segmentation::grabcut::TrimapValue& t);
void
- refine () override;
+ refine() override;
int
- refineOnce () override;
+ refineOnce() override;
void
- fitGMMs () override;
+ fitGMMs() override;
void
- display (int display_type);
+ display(int display_type);
void
- overlayAlpha ();
+ overlayAlpha();
- private:
+private:
void
- buildImages ();
+ buildImages();
// Clouds of various variables that can be displayed for debugging.
pcl::PointCloud<float>::Ptr n_links_image_;
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::setInputCloud
(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
+GrabCutHelper::setInputCloud(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
{
- pcl::GrabCut<pcl::PointXYZRGB>::setInputCloud (cloud);
+ pcl::GrabCut<pcl::PointXYZRGB>::setInputCloud(cloud);
// Reset clouds
- n_links_image_.reset (new pcl::PointCloud<float> (cloud->width, cloud->height, 0));
- t_links_image_.reset (new pcl::segmentation::grabcut::Image (cloud->width, cloud->height));
- gmm_image_.reset (new pcl::segmentation::grabcut::Image (cloud->width, cloud->height));
- alpha_image_.reset (new pcl::PointCloud<float> (cloud->width, cloud->height, 0));
- image_height_1_ = cloud->height-1;
- image_width_1_ = cloud->width-1;
+ n_links_image_.reset(new pcl::PointCloud<float>(cloud->width, cloud->height, 0));
+ t_links_image_.reset(
+ new pcl::segmentation::grabcut::Image(cloud->width, cloud->height));
+ gmm_image_.reset(new pcl::segmentation::grabcut::Image(cloud->width, cloud->height));
+ alpha_image_.reset(new pcl::PointCloud<float>(cloud->width, cloud->height, 0));
+ image_height_1_ = cloud->height - 1;
+ image_width_1_ = cloud->width - 1;
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::setBackgroundPointsIndices (const pcl::PointIndices::ConstPtr& point_indices)
+GrabCutHelper::setBackgroundPointsIndices(
+ const pcl::PointIndices::ConstPtr& point_indices)
{
- pcl::GrabCut<pcl::PointXYZRGB>::setBackgroundPointsIndices (point_indices);
- buildImages ();
+ pcl::GrabCut<pcl::PointXYZRGB>::setBackgroundPointsIndices(point_indices);
+ buildImages();
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::setBackgroundPointsIndices (int x1, int y1, int x2, int y2)
+GrabCutHelper::setBackgroundPointsIndices(int x1, int y1, int x2, int y2)
{
- pcl::PointIndices::Ptr point_indices (new pcl::PointIndices);
- point_indices->indices.reserve (input_->size ());
- if (x1 > x2) std::swap (x1, x2);
- if (y1 > y2) std::swap (y1, y2);
- for (int y = std::max (y1, 0); y <= std::min (static_cast<int> (input_->height -1), y2); ++y)
- for (int x = std::max (x1, 0); x <= std::min (static_cast<int> (input_->width -1), x2); ++x)
- point_indices->indices.push_back (y * input_->width + x);
- setBackgroundPointsIndices (point_indices);
+ pcl::PointIndices::Ptr point_indices(new pcl::PointIndices);
+ point_indices->indices.reserve(input_->size());
+ if (x1 > x2)
+ std::swap(x1, x2);
+ if (y1 > y2)
+ std::swap(y1, y2);
+ x1 = std::max(x1, 0);
+ y1 = std::max(y1, 0);
+ x2 = std::min(static_cast<int>(input_->width - 1), x2);
+ y2 = std::min(static_cast<int>(input_->height - 1), y2);
+ for (int y = y1; y <= y2; ++y)
+ for (int x = x1; x <= x2; ++x)
+ point_indices->indices.push_back(y * input_->width + x);
+ setBackgroundPointsIndices(point_indices);
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::setTrimap(int x1, int y1, int x2, int y2, const pcl::segmentation::grabcut::TrimapValue& t)
+GrabCutHelper::setTrimap(
+ int x1, int y1, int x2, int y2, const pcl::segmentation::grabcut::TrimapValue& t)
{
using namespace pcl::segmentation::grabcut;
- if (x1 > x2) std::swap (x1, x2);
- if (y1 > y2) std::swap (y1, y2);
- for (int y = std::max (y1, 0); y <= std::min (static_cast<int> (image_height_1_), y2); ++y)
- for (int x = std::max (x1, 0); x <= std::min (static_cast<int> (image_width_1_), x2); ++x)
- {
+ if (x1 > x2)
+ std::swap(x1, x2);
+ if (y1 > y2)
+ std::swap(y1, y2);
+ x1 = std::max(x1, 0);
+ y1 = std::max(y1, 0);
+ x2 = std::min(static_cast<int>(image_height_1_), x2);
+ y2 = std::min(static_cast<int>(image_width_1_), y2);
+ for (int y = y1; y <= y2; ++y)
+ for (int x = x1; x <= x2; ++x) {
std::size_t idx = y * input_->width + x;
trimap_[idx] = TrimapUnknown;
// Immediately set the segmentation as well so that the display will update.
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::refine ()
+GrabCutHelper::refine()
{
- pcl::GrabCut<pcl::PointXYZRGB>::refine ();
- buildImages ();
+ pcl::GrabCut<pcl::PointXYZRGB>::refine();
+ buildImages();
}
/////////////////////////////////////////////////////////////////////////////////////////////
int
-GrabCutHelper::refineOnce ()
+GrabCutHelper::refineOnce()
{
- int result = pcl::GrabCut<pcl::PointXYZRGB>::refineOnce ();
- buildImages ();
- return (result);
+ int result = pcl::GrabCut<pcl::PointXYZRGB>::refineOnce();
+ buildImages();
+ return result;
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::fitGMMs ()
+GrabCutHelper::fitGMMs()
{
- pcl::GrabCut<pcl::PointXYZRGB>::fitGMMs ();
- buildImages ();
+ pcl::GrabCut<pcl::PointXYZRGB>::fitGMMs();
+ buildImages();
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::buildImages ()
+GrabCutHelper::buildImages()
{
using namespace pcl::segmentation::grabcut;
- memset (&n_links_image_->points[0], 0, sizeof (float) * n_links_image_->size ());
- for (int y = 0; y < static_cast<int> (image_->height); ++y)
- {
- for (int x = 0; x < static_cast<int> (image_->width); ++x)
- {
+ memset(&n_links_image_->points[0], 0, sizeof(float) * n_links_image_->size());
+ for (int y = 0; y < static_cast<int>(image_->height); ++y) {
+ for (int x = 0; x < static_cast<int>(image_->width); ++x) {
std::size_t index = y * image_->width + x;
- if (x > 0 && y < image_height_1_)
- {
- (*n_links_image_)(x,y) += n_links_[index].weights[0];
- (*n_links_image_)(x-1,y+1) += n_links_[index].weights[0];
+ if (x > 0 && y < image_height_1_) {
+ (*n_links_image_)(x, y) += n_links_[index].weights[0];
+ (*n_links_image_)(x - 1, y + 1) += n_links_[index].weights[0];
}
- if (y < image_height_1_)
- {
- (*n_links_image_)(x,y) += n_links_[index].weights[1];
- (*n_links_image_)(x,y+1) += n_links_[index].weights[1];
+ if (y < image_height_1_) {
+ (*n_links_image_)(x, y) += n_links_[index].weights[1];
+ (*n_links_image_)(x, y + 1) += n_links_[index].weights[1];
}
- if (x < image_width_1_ && y < image_height_1_)
- {
- (*n_links_image_)(x,y) += n_links_[index].weights[2];
- (*n_links_image_)(x+1,y+1) += n_links_[index].weights[2];
+ if (x < image_width_1_ && y < image_height_1_) {
+ (*n_links_image_)(x, y) += n_links_[index].weights[2];
+ (*n_links_image_)(x + 1, y + 1) += n_links_[index].weights[2];
}
- if (x < image_width_1_)
- {
- (*n_links_image_)(x,y) += n_links_[index].weights[3];
- (*n_links_image_)(x+1,y) += n_links_[index].weights[3];
+ if (x < image_width_1_) {
+ (*n_links_image_)(x, y) += n_links_[index].weights[3];
+ (*n_links_image_)(x + 1, y) += n_links_[index].weights[3];
}
// TLinks cloud
- pcl::segmentation::grabcut::Color &tlink_point = t_links_image_->points[index];
- pcl::segmentation::grabcut::Color &gmm_point = gmm_image_->points[index];
- float &alpha_point = alpha_image_->points[index];
- double red = pow (graph_.getSourceEdgeCapacity (index)/L_, 0.25); // red
- double green = pow (graph_.getTargetEdgeCapacity (index)/L_, 0.25); // green
- tlink_point.r = static_cast<float> (red);
- tlink_point.g = static_cast<float> (green);
+ pcl::segmentation::grabcut::Color& tlink_point = t_links_image_->points[index];
+ pcl::segmentation::grabcut::Color& gmm_point = gmm_image_->points[index];
+ float& alpha_point = alpha_image_->points[index];
+ double red = pow(graph_.getSourceEdgeCapacity(index) / L_, 0.25); // red
+ double green = pow(graph_.getTargetEdgeCapacity(index) / L_, 0.25); // green
+ tlink_point.r = static_cast<float>(red);
+ tlink_point.g = static_cast<float>(green);
gmm_point.b = tlink_point.b = 0;
// GMM cloud and Alpha cloud
- if (hard_segmentation_[index] == SegmentationForeground)
- {
- //assert (static_cast<float>(GMM_component_[index]+1)/static_cast<float> (K_) < 1.f);
- gmm_point.r = static_cast<float>(GMM_component_[index]+1)/static_cast<float> (K_);
+ if (hard_segmentation_[index] == SegmentationForeground) {
+ gmm_point.r =
+ static_cast<float>(GMM_component_[index] + 1) / static_cast<float>(K_);
alpha_point = 0;
}
- else
- {
- gmm_point.g = static_cast<float>(GMM_component_[index]+1)/static_cast<float> (K_);
+ else {
+ gmm_point.g =
+ static_cast<float>(GMM_component_[index] + 1) / static_cast<float>(K_);
alpha_point = 0.75;
}
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::display (int display_type)
+GrabCutHelper::display(int display_type)
{
- switch (display_type)
- {
- case 0:
- glDrawPixels (image_->width, image_->height, GL_RGB, GL_FLOAT, &(image_->points[0]));
- break;
-
- case 1:
- glDrawPixels (gmm_image_->width, gmm_image_->height, GL_RGB, GL_FLOAT, &(gmm_image_->points[0]));
- break;
-
- case 2:
- glDrawPixels (n_links_image_->width, n_links_image_->height, GL_LUMINANCE, GL_FLOAT, &(n_links_image_->points[0]));
- break;
-
- case 3:
- glDrawPixels (t_links_image_->width, t_links_image_->height, GL_RGB, GL_FLOAT, &(t_links_image_->points[0]));
- break;
-
- default:
- // Do nothing
- break;
+ switch (display_type) {
+ case 0:
+ glDrawPixels(image_->width, image_->height, GL_RGB, GL_FLOAT, &(image_->points[0]));
+ break;
+
+ case 1:
+ glDrawPixels(gmm_image_->width,
+ gmm_image_->height,
+ GL_RGB,
+ GL_FLOAT,
+ &(gmm_image_->points[0]));
+ break;
+
+ case 2:
+ glDrawPixels(n_links_image_->width,
+ n_links_image_->height,
+ GL_LUMINANCE,
+ GL_FLOAT,
+ &(n_links_image_->points[0]));
+ break;
+
+ case 3:
+ glDrawPixels(t_links_image_->width,
+ t_links_image_->height,
+ GL_RGB,
+ GL_FLOAT,
+ &(t_links_image_->points[0]));
+ break;
+
+ default:
+ // Do nothing
+ break;
}
}
/////////////////////////////////////////////////////////////////////////////////////////////
void
-GrabCutHelper::overlayAlpha ()
+GrabCutHelper::overlayAlpha()
{
- glDrawPixels (alpha_image_->width, alpha_image_->height, GL_ALPHA, GL_FLOAT, &(alpha_image_->points[0]));
+ glDrawPixels(alpha_image_->width,
+ alpha_image_->height,
+ GL_ALPHA,
+ GL_FLOAT,
+ &(alpha_image_->points[0]));
}
/* GUI interface */
/////////////////////////////////////////////////////////////////////////////////////////////
void
-display ()
+display()
{
glClear(GL_COLOR_BUFFER_BIT);
if (display_type == -1)
- glDrawPixels (display_image->width, display_image->height, GL_RGB, GL_FLOAT, &(display_image->points[0]));
+ glDrawPixels(display_image->width,
+ display_image->height,
+ GL_RGB,
+ GL_FLOAT,
+ &(display_image->points[0]));
else
- grabcut.display (display_type);
+ grabcut.display(display_type);
- if (show_mask)
- {
- grabcut.overlayAlpha ();
+ if (show_mask) {
+ grabcut.overlayAlpha();
}
- if (box)
- {
- glColor4f( 1, 1, 1, 1 );
- glBegin( GL_LINE_LOOP );
- glVertex2d( xstart, ystart );
- glVertex2d( xstart, yend );
- glVertex2d( xend, yend );
- glVertex2d( xend, ystart );
+ if (box) {
+ glColor4f(1, 1, 1, 1);
+ glBegin(GL_LINE_LOOP);
+ glVertex2d(xstart, ystart);
+ glVertex2d(xstart, yend);
+ glVertex2d(xend, yend);
+ glVertex2d(xend, ystart);
glEnd();
}
/////////////////////////////////////////////////////////////////////////
void
-idle_callback ()
+idle_callback()
{
int changed = 0;
- if (refining_)
- {
- changed = grabcut.refineOnce ();
- glutPostRedisplay ();
+ if (refining_) {
+ changed = grabcut.refineOnce();
+ glutPostRedisplay();
}
- if (!changed)
- {
+ if (!changed) {
refining_ = false;
- glutIdleFunc (nullptr);
+ glutIdleFunc(nullptr);
}
}
/////////////////////////////////////////////////////////////////////////
void
-motion_callback (int x, int y)
+motion_callback(int x, int y)
{
y = height - y;
- if (box)
- {
- xend = x; yend = y;
- glutPostRedisplay ();
+ if (box) {
+ xend = x;
+ yend = y;
+ glutPostRedisplay();
}
- if (initialized)
- {
+ if (initialized) {
if (left)
- grabcut.setTrimap (x-2,y-2,x+2,y+2,pcl::segmentation::grabcut::TrimapForeground);
+ grabcut.setTrimap(
+ x - 2, y - 2, x + 2, y + 2, pcl::segmentation::grabcut::TrimapForeground);
if (right)
- grabcut.setTrimap (x-2,y-2,x+2,y+2,pcl::segmentation::grabcut::TrimapForeground);
+ grabcut.setTrimap(
+ x - 2, y - 2, x + 2, y + 2, pcl::segmentation::grabcut::TrimapForeground);
- glutPostRedisplay ();
+ glutPostRedisplay();
}
}
void
-mouse_callback (int button, int state, int x, int y)
+mouse_callback(int button, int state, int x, int y)
{
y = height - y;
- switch (button)
- {
- case GLUT_LEFT_BUTTON:
- if (state==GLUT_DOWN)
- {
- left = true;
-
- if (!initialized)
- {
- xstart = x; ystart = y;
- box = true;
- }
- }
+ switch (button) {
+ case GLUT_LEFT_BUTTON:
+ if (state == GLUT_DOWN) {
+ left = true;
- if (state==GLUT_UP)
- {
- left = false;
-
- if (initialized)
- {
- grabcut.refineOnce ();
- glutPostRedisplay ();
- }
- else
- {
- xend = x; yend = y;
- grabcut.setBackgroundPointsIndices (xstart, ystart, xend, yend);
- box = false;
- initialized = true;
- show_mask = true;
- glutPostRedisplay ();
- }
+ if (!initialized) {
+ xstart = x;
+ ystart = y;
+ box = true;
}
- break;
+ }
+
+ if (state == GLUT_UP) {
+ left = false;
- case GLUT_RIGHT_BUTTON:
- if (state==GLUT_DOWN)
- {
- right = true;
+ if (initialized) {
+ grabcut.refineOnce();
+ glutPostRedisplay();
}
- if (state==GLUT_UP)
- {
- right = false;
-
- if (initialized)
- {
- grabcut.refineOnce ();
- glutPostRedisplay ();
- }
+ else {
+ xend = x;
+ yend = y;
+ grabcut.setBackgroundPointsIndices(xstart, ystart, xend, yend);
+ box = false;
+ initialized = true;
+ show_mask = true;
+ glutPostRedisplay();
+ }
+ }
+ break;
+
+ case GLUT_RIGHT_BUTTON:
+ if (state == GLUT_DOWN) {
+ right = true;
+ }
+ if (state == GLUT_UP) {
+ right = false;
+
+ if (initialized) {
+ grabcut.refineOnce();
+ glutPostRedisplay();
}
- break;
+ }
+ break;
- default:
- break;
+ default:
+ break;
}
}
/////////////////////////////////////////////////////////////////////////
void
-keyboard_callback (unsigned char key, int, int)
+keyboard_callback(unsigned char key, int, int)
{
- switch (key)
- {
- case ' ': // space bar show/hide alpha mask
- show_mask = !show_mask;
- break;
- case '0': case 'i': case 'I':// choose the RGB image
- display_type = 0;
- break;
- case '1': case 'g': case 'G':// choose GMM index mask
- display_type = 1;
- break;
- case '2': case 'n': case 'N': // choose N-Link mask
- display_type = 2;
- break;
- case '3': case 't': case 'T': // choose T-Link mask
- display_type = 3;
- break;
- case 'r': // run GrabCut refinement
- refining_ = true;
- glutIdleFunc (idle_callback);
- break;
- case 'o': // run one step of GrabCut refinement
- grabcut.refineOnce ();
- glutPostRedisplay ();
- break;
- case 'l': // rerun the Orchard-Bowman GMM clustering
- grabcut.fitGMMs ();
- glutPostRedisplay ();
- break;
- // case 's': case 'S':
- // save ();
- // break;
- case 'q': case 'Q':
-#if defined (FREEGLUT) || defined (GLUI_OPENGLUT)
- glutLeaveMainLoop ();
+ switch (key) {
+ case ' ': // space bar show/hide alpha mask
+ show_mask = !show_mask;
+ break;
+ case '0':
+ case 'i':
+ case 'I': // choose the RGB image
+ display_type = 0;
+ break;
+ case '1':
+ case 'g':
+ case 'G': // choose GMM index mask
+ display_type = 1;
+ break;
+ case '2':
+ case 'n':
+ case 'N': // choose N-Link mask
+ display_type = 2;
+ break;
+ case '3':
+ case 't':
+ case 'T': // choose T-Link mask
+ display_type = 3;
+ break;
+ case 'r': // run GrabCut refinement
+ refining_ = true;
+ glutIdleFunc(idle_callback);
+ break;
+ case 'o': // run one step of GrabCut refinement
+ grabcut.refineOnce();
+ glutPostRedisplay();
+ break;
+ case 'l': // rerun the Orchard-Bowman GMM clustering
+ grabcut.fitGMMs();
+ glutPostRedisplay();
+ break;
+ // case 's': case 'S':
+ // save ();
+ // break;
+ case 'q':
+ case 'Q':
+#if defined(FREEGLUT) || defined(GLUI_OPENGLUT)
+ glutLeaveMainLoop();
#else
- exit (0);
+ exit(0);
#endif
- break;
- case 27:
- refining_ = false;
- glutIdleFunc(nullptr);
- default:
- break;
+ break;
+ case 27:
+ refining_ = false;
+ glutIdleFunc(nullptr);
+ default:
+ break;
}
- glutPostRedisplay ();
+ glutPostRedisplay();
}
///////////////////////////////////////////////////////////////////////////////////
-int main (int argc, char** argv)
+int
+main(int argc, char** argv)
{
- // Parse the command line arguments for .pcd files
- std::vector<int> parsed_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
- if (parsed_file_indices.empty ())
- {
- pcl::console::print_error ("Need at least an input PCD file (e.g. scene.pcd) to continue!\n\n");
- pcl::console::print_info ("Ideally, need an input file, and two output PCD files, e.g., object.pcd, background.pcd\n");
- return (-1);
+ // Parse the command line arguments for .pcd files
+ std::vector<int> parsed_file_indices =
+ pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
+ if (parsed_file_indices.empty()) {
+ // clang-format off
+ pcl::console::print_error("Need at least an input PCD file (e.g. scene.pcd) to continue!\n\n");
+ pcl::console::print_info("Ideally, need an input file, and two output PCD files, e.g., object.pcd, background.pcd\n");
+ // clang-format on
+ return -1;
}
pcl::PCDReader reader;
// Test the header
pcl::PCLPointCloud2 dummy;
- reader.readHeader (argv[parsed_file_indices[0]], dummy);
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr scene (new pcl::PointCloud<pcl::PointXYZRGB>);
- if (pcl::getFieldIndex (dummy, "rgba") != -1)
- {
- if (pcl::io::loadPCDFile (argv[parsed_file_indices[0]], *scene) < 0)
- {
- pcl::console::print_error (stderr, "[error]\n");
- return (-2);
+ reader.readHeader(argv[parsed_file_indices[0]], dummy);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr scene(new pcl::PointCloud<pcl::PointXYZRGB>);
+ if (pcl::getFieldIndex(dummy, "rgba") != -1) {
+ if (pcl::io::loadPCDFile(argv[parsed_file_indices[0]], *scene) < 0) {
+ pcl::console::print_error(stderr, "[error]\n");
+ return -2;
}
}
- else
- if (pcl::getFieldIndex (dummy, "rgb") != -1)
- {
- if (pcl::io::loadPCDFile (argv[parsed_file_indices[0]], *scene) < 0)
- {
- pcl::console::print_error (stderr, "[error]\n");
- return (-2);
- }
+ else if (pcl::getFieldIndex(dummy, "rgb") != -1) {
+ if (pcl::io::loadPCDFile(argv[parsed_file_indices[0]], *scene) < 0) {
+ pcl::console::print_error(stderr, "[error]\n");
+ return -2;
}
- else
- {
- pcl::console::print_error (stderr, "[No RGB data found!]\n");
- return (-1);
- }
-
- if (scene->isOrganized ())
- {
- pcl::console::print_highlight ("Enabling 2D image viewer mode.\n");
+ }
+ else {
+ pcl::console::print_error(stderr, "[No RGB data found!]\n");
+ return -1;
}
+ if (scene->isOrganized()) {
+ pcl::console::print_highlight("Enabling 2D image viewer mode.\n");
+ }
width = scene->width;
height = scene->height;
display_type = -1;
- display_image.reset (new pcl::segmentation::grabcut::Image (scene->width, scene->height));
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp (new pcl::PointCloud<pcl::PointXYZRGB> (scene->width, scene->height));
-
- if (scene->isOrganized ())
- {
- std::uint32_t height_1 = scene->height -1;
- for (std::size_t i = 0; i < scene->height; ++i)
- {
- for (std::size_t j = 0; j < scene->width; ++j)
- {
- const pcl::PointXYZRGB &p = (*scene) (j,i);
- std::size_t reverse_index = (height_1-i) * scene->width + j;
- display_image->points[reverse_index].r = static_cast<float> (p.r) / 255.0;
- display_image->points[reverse_index].g = static_cast<float> (p.g) / 255.0;
- display_image->points[reverse_index].b = static_cast<float> (p.b) / 255.0;
+ display_image.reset(
+ new pcl::segmentation::grabcut::Image(scene->width, scene->height));
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(
+ new pcl::PointCloud<pcl::PointXYZRGB>(scene->width, scene->height));
+
+ if (scene->isOrganized()) {
+ std::uint32_t height_1 = scene->height - 1;
+ for (std::size_t i = 0; i < scene->height; ++i) {
+ for (std::size_t j = 0; j < scene->width; ++j) {
+ const pcl::PointXYZRGB& p = (*scene)(j, i);
+ std::size_t reverse_index = (height_1 - i) * scene->width + j;
+ display_image->points[reverse_index].r = static_cast<float>(p.r) / 255.0;
+ display_image->points[reverse_index].g = static_cast<float>(p.g) / 255.0;
+ display_image->points[reverse_index].b = static_cast<float>(p.b) / 255.0;
tmp->points[reverse_index] = p;
}
}
}
- grabcut.setInputCloud (tmp);
+ grabcut.setInputCloud(tmp);
- glutInit (&argc,argv);
- glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB);
+ glutInit(&argc, argv);
+ glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
- glutInitWindowSize (width, height);
- glutInitWindowPosition (100,100);
+ glutInitWindowSize(width, height);
+ glutInitWindowPosition(100, 100);
- glutCreateWindow ("GrabCut");
+ glutCreateWindow("GrabCut");
- glOrtho (0,width,0,height,-1,1);
+ glOrtho(0, width, 0, height, -1, 1);
- //set the background color to black (RGBA)
- glClearColor(0.0,0.0,0.0,0.0);
+ // set the background color to black (RGBA)
+ glClearColor(0.0, 0.0, 0.0, 0.0);
glEnable(GL_TEXTURE_2D);
glEnable(GL_BLEND);
- glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
- glutDisplayFunc (display);
- glutMouseFunc (mouse_callback);
- glutMotionFunc (motion_callback);
- glutKeyboardFunc (keyboard_callback);
+ glutDisplayFunc(display);
+ glutMouseFunc(mouse_callback);
+ glutMotionFunc(motion_callback);
+ glutKeyboardFunc(keyboard_callback);
- glutMainLoop ();
+ glutMainLoop();
- return (0);
+ return 0;
}
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
*
* $Id: $
*
- * @author: Koen Buys - KU Leuven
+ * \author: Koen Buys - KU Leuven
*/
#include <pcl/apps/manual_registration.h>
-//QT4
#include <QApplication>
-#include <QMutexLocker>
#include <QEvent>
+#include <QMutexLocker>
#include <QObject>
-// VTK
+#include <vtkCamera.h>
#include <vtkRenderWindow.h>
#include <vtkRendererCollection.h>
-#include <vtkCamera.h>
using namespace pcl;
using namespace std;
//////////////////////////////////////////////////////////////////////////////////////////////////////////
-ManualRegistration::ManualRegistration ()
+ManualRegistration::ManualRegistration()
{
- //Create a timer
- vis_timer_ = new QTimer (this);
- vis_timer_->start (5);//5ms
+ // Create a timer
+ vis_timer_ = new QTimer(this);
+ vis_timer_->start(5); // 5ms
- connect (vis_timer_, SIGNAL (timeout ()), this, SLOT (timeoutSlot()));
+ connect(vis_timer_, SIGNAL(timeout()), this, SLOT(timeoutSlot()));
ui_ = new Ui::MainWindow;
- ui_->setupUi (this);
-
- this->setWindowTitle ("PCL Manual Registration");
+ ui_->setupUi(this);
+
+ this->setWindowTitle("PCL Manual Registration");
// Set up the source window
- vis_src_.reset (new pcl::visualization::PCLVisualizer ("", false));
- ui_->qvtk_widget_src->SetRenderWindow (vis_src_->getRenderWindow ());
- vis_src_->setupInteractor (ui_->qvtk_widget_src->GetInteractor (), ui_->qvtk_widget_src->GetRenderWindow ());
- vis_src_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
- ui_->qvtk_widget_src->update ();
+ vis_src_.reset(new pcl::visualization::PCLVisualizer("", false));
+ ui_->qvtk_widget_src->SetRenderWindow(vis_src_->getRenderWindow());
+ vis_src_->setupInteractor(ui_->qvtk_widget_src->GetInteractor(),
+ ui_->qvtk_widget_src->GetRenderWindow());
+ vis_src_->getInteractorStyle()->setKeyboardModifier(
+ pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+ ui_->qvtk_widget_src->update();
- vis_src_->registerPointPickingCallback (&ManualRegistration::SourcePointPickCallback, *this);
+ vis_src_->registerPointPickingCallback(&ManualRegistration::SourcePointPickCallback,
+ *this);
// Set up the destination window
- vis_dst_.reset (new pcl::visualization::PCLVisualizer ("", false));
- ui_->qvtk_widget_dst->SetRenderWindow (vis_dst_->getRenderWindow ());
- vis_dst_->setupInteractor (ui_->qvtk_widget_dst->GetInteractor (), ui_->qvtk_widget_dst->GetRenderWindow ());
- vis_dst_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
- ui_->qvtk_widget_dst->update ();
-
- vis_dst_->registerPointPickingCallback (&ManualRegistration::DstPointPickCallback, *this);
+ vis_dst_.reset(new pcl::visualization::PCLVisualizer("", false));
+ ui_->qvtk_widget_dst->SetRenderWindow(vis_dst_->getRenderWindow());
+ vis_dst_->setupInteractor(ui_->qvtk_widget_dst->GetInteractor(),
+ ui_->qvtk_widget_dst->GetRenderWindow());
+ vis_dst_->getInteractorStyle()->setKeyboardModifier(
+ pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+ ui_->qvtk_widget_dst->update();
+ vis_dst_->registerPointPickingCallback(&ManualRegistration::DstPointPickCallback,
+ *this);
// Connect all buttons
- connect (ui_->confirmSrcPointButton, SIGNAL(clicked()), this, SLOT(confirmSrcPointPressed()));
- connect (ui_->confirmDstPointButton, SIGNAL(clicked()), this, SLOT(confirmDstPointPressed()));
- connect (ui_->calculateButton, SIGNAL(clicked()), this, SLOT(calculatePressed()));
- connect (ui_->clearButton, SIGNAL(clicked()), this, SLOT(clearPressed()));
- connect (ui_->orthoButton, SIGNAL(stateChanged(int)), this, SLOT(orthoChanged(int)));
- connect (ui_->applyTransformButton, SIGNAL(clicked()), this, SLOT(applyTransformPressed()));
- connect (ui_->refineButton, SIGNAL(clicked()), this, SLOT(refinePressed()));
- connect (ui_->undoButton, SIGNAL(clicked()), this, SLOT(undoPressed()));
- connect (ui_->safeButton, SIGNAL(clicked()), this, SLOT(safePressed()));
+ connect(ui_->confirmSrcPointButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(confirmSrcPointPressed()));
+ connect(ui_->confirmDstPointButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(confirmDstPointPressed()));
+ connect(ui_->calculateButton, SIGNAL(clicked()), this, SLOT(calculatePressed()));
+ connect(ui_->clearButton, SIGNAL(clicked()), this, SLOT(clearPressed()));
+ connect(ui_->orthoButton, SIGNAL(stateChanged(int)), this, SLOT(orthoChanged(int)));
+ connect(ui_->applyTransformButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(applyTransformPressed()));
+ connect(ui_->refineButton, SIGNAL(clicked()), this, SLOT(refinePressed()));
+ connect(ui_->undoButton, SIGNAL(clicked()), this, SLOT(undoPressed()));
+ connect(ui_->safeButton, SIGNAL(clicked()), this, SLOT(safePressed()));
cloud_src_modified_ = true; // first iteration is always a new pointcloud
cloud_dst_modified_ = true;
}
void
-ManualRegistration::SourcePointPickCallback (const pcl::visualization::PointPickingEvent& event, void*)
+ManualRegistration::SourcePointPickCallback(
+ const pcl::visualization::PointPickingEvent& event, void*)
{
// Check to see if we got a valid point. Early exit.
- int idx = event.getPointIndex ();
+ int idx = event.getPointIndex();
if (idx == -1)
return;
// Get the point that was picked
- event.getPoint (src_point_.x, src_point_.y, src_point_.z);
- PCL_INFO ("Src Window: Clicked point %d with X:%f Y:%f Z:%f\n", idx, src_point_.x, src_point_.y, src_point_.z);
+ event.getPoint(src_point_.x, src_point_.y, src_point_.z);
+ PCL_INFO("Src Window: Clicked point %d with X:%f Y:%f Z:%f\n",
+ idx,
+ src_point_.x,
+ src_point_.y,
+ src_point_.z);
src_point_selected_ = true;
}
void
-ManualRegistration::DstPointPickCallback (const pcl::visualization::PointPickingEvent& event, void*)
+ManualRegistration::DstPointPickCallback(
+ const pcl::visualization::PointPickingEvent& event, void*)
{
// Check to see if we got a valid point. Early exit.
- int idx = event.getPointIndex ();
+ int idx = event.getPointIndex();
if (idx == -1)
return;
// Get the point that was picked
- event.getPoint (dst_point_.x, dst_point_.y, dst_point_.z);
- PCL_INFO ("Dst Window: Clicked point %d with X:%f Y:%f Z:%f\n", idx, dst_point_.x, dst_point_.y, dst_point_.z);
+ event.getPoint(dst_point_.x, dst_point_.y, dst_point_.z);
+ PCL_INFO("Dst Window: Clicked point %d with X:%f Y:%f Z:%f\n",
+ idx,
+ dst_point_.x,
+ dst_point_.y,
+ dst_point_.z);
dst_point_selected_ = true;
}
-void
+void
ManualRegistration::confirmSrcPointPressed()
{
- if(src_point_selected_)
- {
+ if (src_point_selected_) {
src_pc_.points.push_back(src_point_);
- PCL_INFO ("Selected %d source points\n", src_pc_.points.size());
+ PCL_INFO("Selected %d source points\n", src_pc_.points.size());
src_point_selected_ = false;
src_pc_.width = src_pc_.points.size();
}
- else
- {
- PCL_INFO ("Please select a point in the source window first\n");
+ else {
+ PCL_INFO("Please select a point in the source window first\n");
}
}
-void
+void
ManualRegistration::confirmDstPointPressed()
{
- if(dst_point_selected_)
- {
+ if (dst_point_selected_) {
dst_pc_.points.push_back(dst_point_);
- PCL_INFO ("Selected %d destination points\n", dst_pc_.points.size());
+ PCL_INFO("Selected %d destination points\n", dst_pc_.points.size());
dst_point_selected_ = false;
dst_pc_.width = dst_pc_.points.size();
}
- else
- {
- PCL_INFO ("Please select a point in the destination window first\n");
+ else {
+ PCL_INFO("Please select a point in the destination window first\n");
}
}
-void
+void
ManualRegistration::calculatePressed()
{
- if(dst_pc_.points.size() != src_pc_.points.size())
- {
- PCL_INFO ("You haven't selected an equal amount of points, please do so\n");
+ if (dst_pc_.points.size() != src_pc_.points.size()) {
+ PCL_INFO("You haven't selected an equal amount of points, please do so\n");
return;
}
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> tfe;
src_point_selected_ = false;
src_pc_.points.clear();
dst_pc_.points.clear();
- src_pc_.height = 1; src_pc_.width = 0;
- dst_pc_.height = 1; dst_pc_.width = 0;
+ src_pc_.height = 1;
+ src_pc_.width = 0;
+ dst_pc_.height = 1;
+ dst_pc_.width = 0;
}
-void
-ManualRegistration::orthoChanged (int state)
+void
+ManualRegistration::orthoChanged(int state)
{
- PCL_INFO ("Ortho state %d\n", state);
- if(state == 0) // Not selected
+ PCL_INFO("Ortho state %d\n", state);
+ if (state == 0) // Not selected
{
- vis_src_->getRenderWindow ()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection(0);
- vis_dst_->getRenderWindow ()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection(0);
+ vis_src_->getRenderWindow()
+ ->GetRenderers()
+ ->GetFirstRenderer()
+ ->GetActiveCamera()
+ ->SetParallelProjection(0);
+ vis_dst_->getRenderWindow()
+ ->GetRenderers()
+ ->GetFirstRenderer()
+ ->GetActiveCamera()
+ ->SetParallelProjection(0);
}
- if(state == 2) // Selected
+ if (state == 2) // Selected
{
- vis_src_->getRenderWindow ()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection(1);
- vis_dst_->getRenderWindow ()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection(1);
+ vis_src_->getRenderWindow()
+ ->GetRenderers()
+ ->GetFirstRenderer()
+ ->GetActiveCamera()
+ ->SetParallelProjection(1);
+ vis_dst_->getRenderWindow()
+ ->GetRenderers()
+ ->GetFirstRenderer()
+ ->GetActiveCamera()
+ ->SetParallelProjection(1);
}
ui_->qvtk_widget_src->update();
ui_->qvtk_widget_dst->update();
}
-//TODO
-void
+// TODO
+void
ManualRegistration::applyTransformPressed()
-{
-}
+{}
void
ManualRegistration::refinePressed()
-{
-}
+{}
void
ManualRegistration::undoPressed()
-{
-}
+{}
void
ManualRegistration::safePressed()
-{
-}
+{}
-void
-ManualRegistration::timeoutSlot ()
+void
+ManualRegistration::timeoutSlot()
{
- if(cloud_src_present_ && cloud_src_modified_)
- {
- if(!vis_src_->updatePointCloud(cloud_src_, "cloud_src_"))
- {
- vis_src_->addPointCloud (cloud_src_, "cloud_src_");
+ if (cloud_src_present_ && cloud_src_modified_) {
+ if (!vis_src_->updatePointCloud(cloud_src_, "cloud_src_")) {
+ vis_src_->addPointCloud(cloud_src_, "cloud_src_");
vis_src_->resetCameraViewpoint("cloud_src_");
}
cloud_src_modified_ = false;
}
- if(cloud_dst_present_ && cloud_dst_modified_)
- {
- if(!vis_dst_->updatePointCloud(cloud_dst_, "cloud_dst_"))
- {
- vis_dst_->addPointCloud (cloud_dst_, "cloud_dst_");
+ if (cloud_dst_present_ && cloud_dst_modified_) {
+ if (!vis_dst_->updatePointCloud(cloud_dst_, "cloud_dst_")) {
+ vis_dst_->addPointCloud(cloud_dst_, "cloud_dst_");
vis_dst_->resetCameraViewpoint("cloud_dst_");
}
cloud_dst_modified_ = false;
}
void
-print_usage ()
+print_usage()
{
- PCL_INFO ("manual_registration cloud1.pcd cloud2.pcd\n");
- PCL_INFO ("\t cloud1 \t source cloud\n");
- PCL_INFO ("\t cloud2 \t destination cloud\n");
+ PCL_INFO("manual_registration cloud1.pcd cloud2.pcd\n");
+ PCL_INFO("\t cloud1 \t source cloud\n");
+ PCL_INFO("\t cloud2 \t destination cloud\n");
}
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
QApplication app(argc, argv);
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_src (new pcl::PointCloud<pcl::PointXYZRGBA>);
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_dst (new pcl::PointCloud<pcl::PointXYZRGBA>);
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_src(
+ new pcl::PointCloud<pcl::PointXYZRGBA>);
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_dst(
+ new pcl::PointCloud<pcl::PointXYZRGBA>);
- if(argc < 3)
- {
- PCL_ERROR ("Incorrect usage\n");
+ if (argc < 3) {
+ PCL_ERROR("Incorrect usage\n");
print_usage();
}
// TODO do this with PCL console
- if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_src) == -1) //* load the file
+ if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(argv[1], *cloud_src) ==
+ -1) //* load the file
{
- PCL_ERROR ("Couldn't read file %s \n", argv[1]);
- return (-1);
+ PCL_ERROR("Couldn't read file %s \n", argv[1]);
+ return -1;
}
- if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_dst) == -1) //* load the file
+ if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(argv[2], *cloud_dst) ==
+ -1) //* load the file
{
- PCL_ERROR ("Couldn't read file %s \n", argv[2]);
- return (-1);
+ PCL_ERROR("Couldn't read file %s \n", argv[2]);
+ return -1;
}
ManualRegistration man_reg;
man_reg.show();
- return (QApplication::exec());
+ return QApplication::exec();
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
+#include <pcl/features/fpfh.h>
#include <pcl/features/multiscale_feature_persistence.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
-#include <pcl/features/fpfh.h>
-
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
using namespace pcl;
-const Eigen::Vector4f subsampling_leaf_size (0.01f, 0.01f, 0.01f, 0.0f);
+const Eigen::Vector4f subsampling_leaf_size(0.01f, 0.01f, 0.01f, 0.0f);
const float normal_estimation_search_radius = 0.05f;
-
void
-subsampleAndCalculateNormals (PointCloud<PointXYZ>::Ptr &cloud,
- PointCloud<PointXYZ>::Ptr &cloud_subsampled,
- PointCloud<Normal>::Ptr &cloud_subsampled_normals)
+subsampleAndCalculateNormals(PointCloud<PointXYZ>::Ptr& cloud,
+ PointCloud<PointXYZ>::Ptr& cloud_subsampled,
+ PointCloud<Normal>::Ptr& cloud_subsampled_normals)
{
- cloud_subsampled = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ> ());
+ cloud_subsampled = PointCloud<PointXYZ>::Ptr(new PointCloud<PointXYZ>());
VoxelGrid<PointXYZ> subsampling_filter;
- subsampling_filter.setInputCloud (cloud);
- subsampling_filter.setLeafSize (subsampling_leaf_size);
- subsampling_filter.filter (*cloud_subsampled);
+ subsampling_filter.setInputCloud(cloud);
+ subsampling_filter.setLeafSize(subsampling_leaf_size);
+ subsampling_filter.filter(*cloud_subsampled);
- cloud_subsampled_normals = PointCloud<Normal>::Ptr (new PointCloud<Normal> ());
+ cloud_subsampled_normals = PointCloud<Normal>::Ptr(new PointCloud<Normal>());
NormalEstimation<PointXYZ, Normal> normal_estimation_filter;
- normal_estimation_filter.setInputCloud (cloud_subsampled);
- pcl::search::KdTree<PointXYZ>::Ptr search_tree
(new pcl::search::KdTree<PointXYZ>);
- normal_estimation_filter.setSearchMethod (search_tree);
- normal_estimation_filter.setRadiusSearch (normal_estimation_search_radius);
- normal_estimation_filter.compute (*cloud_subsampled_normals);
+ normal_estimation_filter.setInputCloud(cloud_subsampled);
+ pcl::search::KdTree<PointXYZ>::Ptr search_tree(new pcl::search::KdTree<PointXYZ>);
+ normal_estimation_filter.setSearchMethod(search_tree);
+ normal_estimation_filter.setRadiusSearch(normal_estimation_search_radius);
+ normal_estimation_filter.compute(*cloud_subsampled_normals);
}
-
int
-main (int argc, char **argv)
+main(int argc, char** argv)
{
- if (argc != 2)
- {
- PCL_ERROR ("Syntax: ./multiscale_feature_persistence_example [path_to_cloud.pcl]\n");
+ if (argc != 2) {
+ PCL_ERROR("Syntax: ./multiscale_feature_persistence_example [path_to_cloud.pcl]\n");
return -1;
}
- PointCloud<PointXYZ>::Ptr cloud_scene (new PointCloud<PointXYZ> ());
+ PointCloud<PointXYZ>::Ptr cloud_scene(new PointCloud<PointXYZ>());
PCDReader reader;
- reader.read (argv[1], *cloud_scene);
+ reader.read(argv[1], *cloud_scene);
PointCloud<PointXYZ>::Ptr cloud_subsampled;
PointCloud<Normal>::Ptr cloud_subsampled_normals;
- subsampleAndCalculateNormals (cloud_scene, cloud_subsampled, cloud_subsampled_normals);
-
- PCL_INFO ("STATS:\ninitial point cloud size: %u\nsubsampled point cloud size: %u\n", cloud_scene->points.size (), cloud_subsampled->points.size ());
- visualization::CloudViewer viewer ("Multiscale Feature Persistence Example Visualization");
- viewer.showCloud (cloud_scene, "scene");
+ subsampleAndCalculateNormals(cloud_scene, cloud_subsampled, cloud_subsampled_normals);
+ PCL_INFO("STATS:\ninitial point cloud size: %u\nsubsampled point cloud size: %u\n",
+ cloud_scene->points.size(),
+ cloud_subsampled->points.size());
+ visualization::CloudViewer viewer(
+ "Multiscale Feature Persistence Example Visualization");
+ viewer.showCloud(cloud_scene, "scene");
MultiscaleFeaturePersistence<PointXYZ, FPFHSignature33> feature_persistence;
std::vector<float> scale_values;
for (float x = 2.0f; x < 3.6f; x += 0.35f)
- scale_values.push_back (x / 100.0f);
- feature_persistence.setScalesVector (scale_values);
- feature_persistence.setAlpha (1.3f);
- FPFHEstimation<PointXYZ, Normal, FPFHSignature33>::Ptr fpfh_estimation (new FPFHEstimation<PointXYZ, Normal, FPFHSignature33> ());
- fpfh_estimation->setInputCloud (cloud_subsampled);
- fpfh_estimation->setInputNormals (cloud_subsampled_normals);
- pcl::search::KdTree<PointXYZ>::Ptr tree (new pcl::search::KdTree<PointXYZ> ());
- fpfh_estimation->setSearchMethod (tree);
- feature_persistence.setFeatureEstimator (fpfh_estimation);
- feature_persistence.setDistanceMetric (pcl::CS);
-
- PointCloud<FPFHSignature33>::Ptr output_features (new PointCloud<FPFHSignature33> ());
- auto output_indices = pcl::make_shared<std::vector<int>> ();
- feature_persistence.determinePersistentFeatures (*output_features, output_indices);
-
- PCL_INFO ("persistent features cloud size: %u\n", output_features->points.size ());
+ scale_values.push_back(x / 100.0f);
+ feature_persistence.setScalesVector(scale_values);
+ feature_persistence.setAlpha(1.3f);
+ FPFHEstimation<PointXYZ, Normal, FPFHSignature33>::Ptr fpfh_estimation(
+ new FPFHEstimation<PointXYZ, Normal, FPFHSignature33>());
+ fpfh_estimation->setInputCloud(cloud_subsampled);
+ fpfh_estimation->setInputNormals(cloud_subsampled_normals);
+ pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>());
+ fpfh_estimation->setSearchMethod(tree);
+ feature_persistence.setFeatureEstimator(fpfh_estimation);
+ feature_persistence.setDistanceMetric(pcl::CS);
+
+ PointCloud<FPFHSignature33>::Ptr output_features(new PointCloud<FPFHSignature33>());
+ auto output_indices = pcl::make_shared<std::vector<int>>();
+ feature_persistence.determinePersistentFeatures(*output_features, output_indices);
+
+ PCL_INFO("persistent features cloud size: %u\n", output_features->points.size());
ExtractIndices<PointXYZ> extract_indices_filter;
- extract_indices_filter.setInputCloud (cloud_subsampled);
- extract_indices_filter.setIndices (output_indices);
- PointCloud<PointXYZ>::Ptr persistent_features_locations (new PointCloud<PointXYZ> ());
- extract_indices_filter.filter (*persistent_features_locations);
-
- viewer.showCloud (persistent_features_locations, "persistent features");
- PCL_INFO ("Persistent features have been computed. Waiting for the user to quit the visualization window.\n");
+ extract_indices_filter.setInputCloud(cloud_subsampled);
+ extract_indices_filter.setIndices(output_indices);
+ PointCloud<PointXYZ>::Ptr persistent_features_locations(new PointCloud<PointXYZ>());
+ extract_indices_filter.filter(*persistent_features_locations);
-// io::savePCDFileASCII ("persistent_features.pcd", *persistent_features_locations);
-// PCL_INFO ("\nPersistent feature locations written to persistent_features.pcd\n");
+ viewer.showCloud(persistent_features_locations, "persistent features");
+ PCL_INFO("Persistent features have been computed. Waiting for the user to quit the "
+ "visualization window.\n");
- while (!viewer.wasStopped (50)) {}
+ while (!viewer.wasStopped(50)) {
+ }
- return (0);
+ return 0;
}
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
#define SHOW_FPS 1
#include <pcl/apps/timer.h>
-#include <pcl/common/common.h>
#include <pcl/common/angles.h>
+#include <pcl/common/common.h>
#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/filters/extract_indices.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/agast_2d.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
+#include <pcl/visualization/pcl_visualizer.h>
#include <mutex>
#include <thread>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
-class AGASTDemo
-{
- public:
- using Cloud = PointCloud<PointT>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- AGASTDemo (Grabber& grabber)
- : cloud_viewer_ ("AGAST 2D Keypoints -- PointCloud")
- , grabber_ (grabber)
- , image_viewer_ ("AGAST 2D Keypoints -- Image")
- , bmax_ (255)
- , threshold_ (30)
- , detector_type_ (0)
- {
+class AGASTDemo {
+public:
+ using Cloud = PointCloud<PointT>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ AGASTDemo(Grabber& grabber)
+ : cloud_viewer_("AGAST 2D Keypoints -- PointCloud")
+ , grabber_(grabber)
+ , image_viewer_("AGAST 2D Keypoints -- Image")
+ , bmax_(255)
+ , threshold_(30)
+ , detector_type_(0)
+ {}
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ cloud_callback(const CloudConstPtr& cloud)
+ {
+ FPS_CALC("cloud callback");
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+
+ // Compute AGAST keypoints
+ AgastKeypoint2D<PointT> agast;
+ agast.setNonMaxSuppression(true);
+ agast.setThreshold(threshold_);
+ agast.setMaxDataValue(bmax_);
+ agast.setInputCloud(cloud);
+
+ keypoints_.reset(new PointCloud<KeyPointT>);
+
+ // Select the detector type
+ switch (detector_type_) {
+ case 1:
+ default: {
+ timer_.reset();
+ pcl::keypoints::agast::AgastDetector7_12s::Ptr detector(
+ new pcl::keypoints::agast::AgastDetector7_12s(
+ cloud->width, cloud->height, threshold_, bmax_));
+ agast.setAgastDetector(detector);
+ agast.compute(*keypoints_);
+ PCL_DEBUG("AGAST 7_12s computation took %f ms.\n", timer_.getTime());
+ break;
}
-
- /////////////////////////////////////////////////////////////////////////
- void
- cloud_callback (const CloudConstPtr& cloud)
- {
- FPS_CALC ("cloud callback");
- std::lock_guard<std::mutex> lock (cloud_mutex_);
-
- // Compute AGAST keypoints
- AgastKeypoint2D<PointT> agast;
- agast.setNonMaxSuppression (true);
- agast.setThreshold (threshold_);
- agast.setMaxDataValue (bmax_);
- agast.setInputCloud (cloud);
-
- keypoints_.reset (new PointCloud<KeyPointT>);
-
- // Select the detector type
- switch (detector_type_)
- {
- case 1:
- default:
- {
- timer_.reset ();
- pcl::keypoints::agast::AgastDetector7_12s::Ptr detector (new pcl::keypoints::agast::AgastDetector7_12s (cloud->width, cloud->height, threshold_, bmax_));
- agast.setAgastDetector (detector);
- agast.compute (*keypoints_);
- PCL_DEBUG ("AGAST 7_12s computation took %f ms.\n", timer_.getTime ());
- break;
- }
- case 2:
- {
- timer_.reset ();
- pcl::keypoints::agast::AgastDetector5_8::Ptr detector (new pcl::keypoints::agast::AgastDetector5_8 (cloud->width, cloud->height, threshold_, bmax_));
- agast.setAgastDetector (detector);
- agast.compute (*keypoints_);
- PCL_DEBUG ("AGAST 5_8 computation took %f ms.\n", timer_.getTime ());
- break;
- }
- case 3:
- {
- timer_.reset ();
- pcl::keypoints::agast::OastDetector9_16::Ptr detector (new pcl::keypoints::agast::OastDetector9_16 (cloud->width, cloud->height, threshold_, bmax_));
- agast.setAgastDetector (detector);
- agast.compute (*keypoints_);
- PCL_DEBUG ("OAST 9_6 computation took %f ms.\n", timer_.getTime ());
- break;
- }
- }
- cloud_ = cloud;
+ case 2: {
+ timer_.reset();
+ pcl::keypoints::agast::AgastDetector5_8::Ptr detector(
+ new pcl::keypoints::agast::AgastDetector5_8(
+ cloud->width, cloud->height, threshold_, bmax_));
+ agast.setAgastDetector(detector);
+ agast.compute(*keypoints_);
+ PCL_DEBUG("AGAST 5_8 computation took %f ms.\n", timer_.getTime());
+ break;
+ }
+ case 3: {
+ timer_.reset();
+ pcl::keypoints::agast::OastDetector9_16::Ptr detector(
+ new pcl::keypoints::agast::OastDetector9_16(
+ cloud->width, cloud->height, threshold_, bmax_));
+ agast.setAgastDetector(detector);
+ agast.compute(*keypoints_);
+ PCL_DEBUG("OAST 9_6 computation took %f ms.\n", timer_.getTime());
+ break;
+ }
+ }
+ cloud_ = cloud;
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ keyboard_callback(const pcl::visualization::KeyboardEvent& event, void* cookie)
+ {
+ AGASTDemo* obj = static_cast<AGASTDemo*>(cookie);
+
+ if (event.getKeyCode()) {
+ std::stringstream ss;
+ ss << event.getKeyCode();
+ obj->detector_type_ = atoi(ss.str().c_str());
+ return;
}
- /////////////////////////////////////////////////////////////////////////
- void
- keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* cookie)
- {
- AGASTDemo* obj = static_cast<AGASTDemo*> (cookie);
-
- if (event.getKeyCode ())
- {
- std::stringstream ss; ss << event.getKeyCode ();
- obj->detector_type_ = atoi (ss.str ().c_str ());
+ if (event.getKeySym() == "Up") {
+ if (obj->threshold_ <= 0.9) {
+ PCL_INFO("[keyboard_callback] Increase AGAST threshold from %f to %f.\n",
+ obj->threshold_,
+ obj->threshold_ + 0.01);
+ obj->threshold_ += 0.01;
return;
}
+ PCL_INFO("[keyboard_callback] Increase AGAST threshold from %f to %f.\n",
+ obj->threshold_,
+ obj->threshold_ + 1);
+ obj->threshold_ += 1;
+ return;
+ }
- if (event.getKeySym () == "Up")
- {
- if (obj->threshold_ <= 0.9)
- {
- PCL_INFO ("[keyboard_callback] Increase AGAST threshold from %f to %f.\n", obj->threshold_, obj->threshold_ + 0.01);
- obj->threshold_ += 0.01;
- return;
- }
- PCL_INFO ("[keyboard_callback] Increase AGAST threshold from %f to %f.\n", obj->threshold_, obj->threshold_ + 1);
- obj->threshold_ += 1;
+ if (event.getKeySym() == "Down") {
+ if (obj->threshold_ <= 0)
return;
- }
-
- if (event.getKeySym () == "Down")
- {
- if (obj->threshold_ <= 0)
- return;
- if (obj->threshold_ <= 1)
- {
- PCL_INFO ("[keyboard_callback] Decrease AGAST threshold from %f to %f.\n", obj->threshold_, obj->threshold_ - 0.01);
- obj->threshold_ -= 0.01;
- return;
- }
- PCL_INFO ("[keyboard_callback] Decrease AGAST threshold from %f to %f.\n", obj->threshold_, obj->threshold_ - 1);
- obj->threshold_ -= 1;
+ if (obj->threshold_ <= 1) {
+ PCL_INFO("[keyboard_callback] Decrease AGAST threshold from %f to %f.\n",
+ obj->threshold_,
+ obj->threshold_ - 0.01);
+ obj->threshold_ -= 0.01;
return;
}
+ PCL_INFO("[keyboard_callback] Decrease AGAST threshold from %f to %f.\n",
+ obj->threshold_,
+ obj->threshold_ - 1);
+ obj->threshold_ -= 1;
+ return;
+ }
- if (event.getKeySym () == "Right")
- {
- PCL_INFO ("[keyboard_callback] Increase AGAST BMAX from %f to %f.\n", obj->bmax_, obj->bmax_ + 1);
- obj->bmax_ += 1;
- return;
- }
+ if (event.getKeySym() == "Right") {
+ PCL_INFO("[keyboard_callback] Increase AGAST BMAX from %f to %f.\n",
+ obj->bmax_,
+ obj->bmax_ + 1);
+ obj->bmax_ += 1;
+ return;
+ }
- if (event.getKeySym () == "Left")
- {
- if (obj->bmax_ <= 0)
- return;
- PCL_INFO ("[keyboard_callback] Decrease AGAST BMAX from %f to %f.\n", obj->bmax_, obj->bmax_ - 1);
- obj->bmax_ -= 1;
+ if (event.getKeySym() == "Left") {
+ if (obj->bmax_ <= 0)
return;
- }
+ PCL_INFO("[keyboard_callback] Decrease AGAST BMAX from %f to %f.\n",
+ obj->bmax_,
+ obj->bmax_ - 1);
+ obj->bmax_ -= 1;
+ return;
}
-
- /////////////////////////////////////////////////////////////////////////
- void
- init ()
- {
- std::function<void (const CloudConstPtr&) > cloud_cb = [this] (const CloudConstPtr& cloud) { cloud_callback (cloud); };
- cloud_connection = grabber_.registerCallback (cloud_cb);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ init()
+ {
+ std::function<void(const CloudConstPtr&)> cloud_cb =
+ [this](const CloudConstPtr& cloud) { cloud_callback(cloud); };
+ cloud_connection = grabber_.registerCallback(cloud_cb);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ string
+ getStrBool(bool state)
+ {
+ stringstream ss;
+ ss << state;
+ return ss.str();
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ get3DKeypoints(const CloudConstPtr& cloud,
+ const PointCloud<KeyPointT>::Ptr& keypoints,
+ PointCloud<PointT>& keypoints3d)
+ {
+ if (!cloud || !keypoints || cloud->points.empty() || keypoints->points.empty())
+ return;
+
+ keypoints3d.resize(keypoints->size());
+ keypoints3d.width = keypoints->width;
+ keypoints3d.height = keypoints->height;
+ keypoints3d.is_dense = true;
+
+ std::size_t j = 0;
+ for (std::size_t i = 0; i < keypoints->size(); ++i) {
+ const PointT& pt =
+ (*cloud)(static_cast<long unsigned int>(keypoints->points[i].u),
+ static_cast<long unsigned int>(keypoints->points[i].v));
+ if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z))
+ continue;
+
+ keypoints3d.points[j].x = pt.x;
+ keypoints3d.points[j].y = pt.y;
+ keypoints3d.points[j].z = pt.z;
+ ++j;
}
- /////////////////////////////////////////////////////////////////////////
- string
- getStrBool (bool state)
- {
- stringstream ss;
- ss << state;
- return (ss.str ());
+ if (j != keypoints->size()) {
+ keypoints3d.resize(j);
+ keypoints3d.width = j;
+ keypoints3d.height = 1;
}
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- get3DKeypoints (
- const CloudConstPtr &cloud,
- const PointCloud<KeyPointT>::Ptr &keypoints, PointCloud<PointT> &keypoints3d)
- {
- if (!cloud || !keypoints || cloud->points.empty () || keypoints->points.empty ())
- return;
+ /////////////////////////////////////////////////////////////////////////
+ void
+ run()
+ {
+ cloud_viewer_.registerKeyboardCallback(
+ &AGASTDemo::keyboard_callback, *this, static_cast<AGASTDemo*>(this));
+ image_viewer_.registerKeyboardCallback(
+ &AGASTDemo::keyboard_callback, *this, static_cast<AGASTDemo*>(this));
- keypoints3d.resize (keypoints->size ());
- keypoints3d.width = keypoints->width;
- keypoints3d.height = keypoints->height;
- keypoints3d.is_dense = true;
-
- std::size_t j = 0;
- for (std::size_t i = 0; i < keypoints->size (); ++i)
- {
- const PointT &pt = (*cloud)(static_cast<long unsigned int> (keypoints->points[i].u),
- static_cast<long unsigned int> (keypoints->points[i].v));
- if (!std::isfinite (pt.x) || !std::isfinite (pt.y) || !std::isfinite (pt.z))
- continue;
-
- keypoints3d.points[j].x = pt.x;
- keypoints3d.points[j].y = pt.y;
- keypoints3d.points[j].z = pt.z;
- ++j;
- }
+ grabber_.start();
+
+ bool image_init = false, cloud_init = false;
+ bool keypts = true;
- if (j != keypoints->size ())
- {
- keypoints3d.resize (j);
- keypoints3d.width = j;
- keypoints3d.height = 1;
+ CloudPtr keypoints3d(new Cloud);
+
+ while (!cloud_viewer_.wasStopped() && !image_viewer_.wasStopped()) {
+ PointCloud<KeyPointT>::Ptr keypoints;
+ CloudConstPtr cloud;
+
+ if (cloud_mutex_.try_lock()) {
+ cloud_.swap(cloud);
+ keypoints_.swap(keypoints);
+
+ cloud_mutex_.unlock();
}
- }
-
- /////////////////////////////////////////////////////////////////////////
- void
- run ()
- {
- cloud_viewer_.registerKeyboardCallback (&AGASTDemo::keyboard_callback, *this, static_cast<AGASTDemo*> (this));
- image_viewer_.registerKeyboardCallback (&AGASTDemo::keyboard_callback, *this, static_cast<AGASTDemo*> (this));
-
- grabber_.start ();
-
- bool image_init = false, cloud_init = false;
- bool keypts = true;
-
- CloudPtr keypoints3d (new Cloud);
-
- while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ())
- {
- PointCloud<KeyPointT>::Ptr keypoints;
- CloudConstPtr cloud;
-
- if (cloud_mutex_.try_lock ())
- {
- cloud_.swap (cloud);
- keypoints_.swap (keypoints);
-
- cloud_mutex_.unlock ();
- }
- if (cloud)
- {
- if (!cloud_init)
- {
- cloud_viewer_.setPosition (0, 0);
- cloud_viewer_.setSize (cloud->width, cloud->height);
- cloud_init = true;
- }
+ if (cloud) {
+ if (!cloud_init) {
+ cloud_viewer_.setPosition(0, 0);
+ cloud_viewer_.setSize(cloud->width, cloud->height);
+ cloud_init = true;
+ }
- if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud"))
- {
- cloud_viewer_.addPointCloud (cloud, "OpenNICloud");
- cloud_viewer_.resetCameraViewpoint ("OpenNICloud");
- }
+ if (!cloud_viewer_.updatePointCloud(cloud, "OpenNICloud")) {
+ cloud_viewer_.addPointCloud(cloud, "OpenNICloud");
+ cloud_viewer_.resetCameraViewpoint("OpenNICloud");
+ }
- if (!image_init)
- {
- image_viewer_.setPosition (cloud->width, 0);
- image_viewer_.setSize (cloud->width, cloud->height);
- image_init = true;
- }
+ if (!image_init) {
+ image_viewer_.setPosition(cloud->width, 0);
+ image_viewer_.setSize(cloud->width, cloud->height);
+ image_init = true;
+ }
- image_viewer_.addRGBImage<PointT> (cloud);
-
- if (keypoints && !keypoints->empty ())
- {
- image_viewer_.removeLayer (getStrBool (keypts));
- for (std::size_t i = 0; i < keypoints->size (); ++i)
- {
- int u = int (keypoints->points[i].u);
- int v = int (keypoints->points[i].v);
- image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 10, getStrBool (!keypts));
- }
- keypts = !keypts;
-
- get3DKeypoints (cloud, keypoints, *keypoints3d);
- visualization::PointCloudColorHandlerCustom<PointT> blue (keypoints3d, 0, 0, 255);
- if (!cloud_viewer_.updatePointCloud (keypoints3d, blue, "keypoints"))
- cloud_viewer_.addPointCloud (keypoints3d, blue, "keypoints");
- cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 20, "keypoints");
- cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
+ image_viewer_.addRGBImage<PointT>(cloud);
+
+ if (keypoints && !keypoints->empty()) {
+ image_viewer_.removeLayer(getStrBool(keypts));
+ for (std::size_t i = 0; i < keypoints->size(); ++i) {
+ int u = int(keypoints->points[i].u);
+ int v = int(keypoints->points[i].v);
+ image_viewer_.markPoint(u,
+ v,
+ visualization::red_color,
+ visualization::blue_color,
+ 10,
+ getStrBool(!keypts));
}
+ keypts = !keypts;
+
+ get3DKeypoints(cloud, keypoints, *keypoints3d);
+ visualization::PointCloudColorHandlerCustom<PointT> blue(
+ keypoints3d, 0, 0, 255);
+ if (!cloud_viewer_.updatePointCloud(keypoints3d, blue, "keypoints"))
+ cloud_viewer_.addPointCloud(keypoints3d, blue, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties(
+ visualization::PCL_VISUALIZER_POINT_SIZE, 20, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties(
+ visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
}
-
- cloud_viewer_.spinOnce ();
- image_viewer_.spinOnce ();
- std::this_thread::sleep_for(100us);
}
- grabber_.stop ();
- cloud_connection.disconnect ();
+ cloud_viewer_.spinOnce();
+ image_viewer_.spinOnce();
+ std::this_thread::sleep_for(100us);
}
-
- visualization::PCLVisualizer cloud_viewer_;
- Grabber& grabber_;
- std::mutex cloud_mutex_;
- CloudConstPtr cloud_;
-
- visualization::ImageViewer image_viewer_;
-
- PointCloud<KeyPointT>::Ptr keypoints_;
-
- double bmax_;
- double threshold_;
- int detector_type_;
- private:
- boost::signals2::connection cloud_connection;
- StopWatch timer_;
+
+ grabber_.stop();
+ cloud_connection.disconnect();
+ }
+
+ visualization::PCLVisualizer cloud_viewer_;
+ Grabber& grabber_;
+ std::mutex cloud_mutex_;
+ CloudConstPtr cloud_;
+
+ visualization::ImageViewer image_viewer_;
+
+ PointCloud<KeyPointT>::Ptr keypoints_;
+
+ double bmax_;
+ double threshold_;
+ int detector_type_;
+
+private:
+ boost::signals2::connection cloud_connection;
+ StopWatch timer_;
};
/* ---[ */
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
bool debug = false;
- pcl::console::parse_argument (argc, argv, "-debug", debug);
+ pcl::console::parse_argument(argc, argv, "-debug", debug);
if (debug)
- pcl::console::setVerbosityLevel (pcl::console::L_DEBUG);
+ pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
else
- pcl::console::setVerbosityLevel (pcl::console::L_INFO);
+ pcl::console::setVerbosityLevel(pcl::console::L_INFO);
+
+ string device_id("#1");
+ OpenNIGrabber grabber(device_id);
+ AGASTDemo<PointXYZRGBA> openni_viewer(grabber);
- string device_id ("#1");
- OpenNIGrabber grabber (device_id);
- AGASTDemo<PointXYZRGBA> openni_viewer (grabber);
+ openni_viewer.init();
+ openni_viewer.run();
- openni_viewer.init ();
- openni_viewer.run ();
-
- return (0);
+ return 0;
}
/* ]--- */
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
#define SHOW_FPS 1
#include <pcl/apps/timer.h>
-#include <pcl/common/common.h>
#include <pcl/common/angles.h>
+#include <pcl/common/common.h>
#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/filters/extract_indices.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/brisk_2d.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
+#include <pcl/visualization/pcl_visualizer.h>
#include <mutex>
#include <thread>
using KeyPointT = PointWithScale;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-class BRISKDemo
-{
- public:
- using Cloud = PointCloud<PointT>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
-
- BRISKDemo (Grabber& grabber)
- : cloud_viewer_ ("BRISK 2D Keypoints -- PointCloud")
- , grabber_ (grabber)
- , image_viewer_ ("BRISK 2D Keypoints -- Image")
- {
+class BRISKDemo {
+public:
+ using Cloud = PointCloud<PointT>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ BRISKDemo(Grabber& grabber)
+ : cloud_viewer_("BRISK 2D Keypoints -- PointCloud")
+ , grabber_(grabber)
+ , image_viewer_("BRISK 2D Keypoints -- Image")
+ {}
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ cloud_callback(const CloudConstPtr& cloud)
+ {
+ FPS_CALC("cloud callback");
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ cloud_ = cloud;
+
+ // Compute BRISK keypoints
+ BriskKeypoint2D<PointT> brisk;
+ brisk.setThreshold(60);
+ brisk.setOctaves(4);
+ brisk.setInputCloud(cloud);
+
+ keypoints_.reset(new PointCloud<KeyPointT>);
+ brisk.compute(*keypoints_);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ init()
+ {
+ std::function<void(const CloudConstPtr&)> cloud_cb =
+ [this](const CloudConstPtr& cloud) { cloud_callback(cloud); };
+ cloud_connection = grabber_.registerCallback(cloud_cb);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ string
+ getStrBool(bool state)
+ {
+ stringstream ss;
+ ss << state;
+ return ss.str();
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ inline PointT
+ bilinearInterpolation(const CloudConstPtr& cloud, float x, float y)
+ {
+ int u = int(x);
+ int v = int(y);
+
+ PointT pt;
+ pt.x = pt.y = pt.z = 0;
+
+ const PointT& p1 = (*cloud)(u, v);
+ const PointT& p2 = (*cloud)(u + 1, v);
+ const PointT& p3 = (*cloud)(u, v + 1);
+ const PointT& p4 = (*cloud)(u + 1, v + 1);
+
+ float fx = x - float(u), fy = y - float(v);
+ float fx1 = 1.0f - fx, fy1 = 1.0f - fy;
+
+ float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy;
+ float weight = 0;
+
+ if (isFinite(p1)) {
+ pt.x += p1.x * w1;
+ pt.y += p1.y * w1;
+ pt.z += p1.z * w1;
+ weight += w1;
+ }
+ if (isFinite(p2)) {
+ pt.x += p2.x * w2;
+ pt.y += p2.y * w2;
+ pt.z += p2.z * w2;
+ weight += w2;
+ }
+ if (isFinite(p3)) {
+ pt.x += p3.x * w3;
+ pt.y += p3.y * w3;
+ pt.z += p3.z * w3;
+ weight += w3;
+ }
+ if (isFinite(p4)) {
+ pt.x += p4.x * w4;
+ pt.y += p4.y * w4;
+ pt.z += p4.z * w4;
+ weight += w4;
}
- /////////////////////////////////////////////////////////////////////////
- void
- cloud_callback (const CloudConstPtr& cloud)
- {
- FPS_CALC ("cloud callback");
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- cloud_ = cloud;
-
- // Compute BRISK keypoints
- BriskKeypoint2D<PointT> brisk;
- brisk.setThreshold (60);
- brisk.setOctaves (4);
- brisk.setInputCloud (cloud);
-
- keypoints_.reset (new PointCloud<KeyPointT>);
- brisk.compute (*keypoints_);
+ if (weight == 0)
+ pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+ else {
+ weight = 1.0f / weight;
+ pt.x *= weight;
+ pt.y *= weight;
+ pt.z *= weight;
}
- /////////////////////////////////////////////////////////////////////////
- void
- init ()
- {
- std::function<void (const CloudConstPtr&) > cloud_cb = [this] (const CloudConstPtr& cloud) { cloud_callback (cloud); };
- cloud_connection = grabber_.registerCallback (cloud_cb);
+ return pt;
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ get3DKeypoints(const CloudConstPtr& cloud,
+ const PointCloud<KeyPointT>::Ptr& keypoints,
+ PointCloud<PointT>& keypoints3d)
+ {
+ keypoints3d.resize(keypoints->size());
+ keypoints3d.width = keypoints->width;
+ keypoints3d.height = keypoints->height;
+ keypoints3d.is_dense = true;
+
+ std::size_t j = 0;
+ for (std::size_t i = 0; i < keypoints->size(); ++i) {
+ PointT pt =
+ bilinearInterpolation(cloud, keypoints->points[i].x, keypoints->points[i].y);
+
+ keypoints3d.points[j].x = pt.x;
+ keypoints3d.points[j].y = pt.y;
+ keypoints3d.points[j].z = pt.z;
+ ++j;
}
- /////////////////////////////////////////////////////////////////////////
- string
- getStrBool (bool state)
- {
- stringstream ss;
- ss << state;
- return (ss.str ());
+ if (j != keypoints->size()) {
+ keypoints3d.resize(j);
+ keypoints3d.width = j;
+ keypoints3d.height = 1;
}
+ }
- /////////////////////////////////////////////////////////////////////////
- inline PointT
- bilinearInterpolation (const CloudConstPtr &cloud, float x, float y)
- {
- int u = int (x);
- int v = int (y);
-
- PointT pt;
- pt.x = pt.y = pt.z = 0;
-
- const PointT &p1 = (*cloud)(u, v);
- const PointT &p2 = (*cloud)(u+1, v);
- const PointT &p3 = (*cloud)(u, v+1);
- const PointT &p4 = (*cloud)(u+1, v+1);
-
- float fx = x - float (u), fy = y - float (v);
- float fx1 = 1.0f - fx, fy1 = 1.0f - fy;
-
- float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy;
- float weight = 0;
-
- if (isFinite (p1))
- {
- pt.x += p1.x * w1;
- pt.y += p1.y * w1;
- pt.z += p1.z * w1;
- weight += w1;
- }
- if (isFinite (p2))
- {
- pt.x += p2.x * w2;
- pt.y += p2.y * w2;
- pt.z += p2.z * w2;
- weight += w2;
- }
- if (isFinite (p3))
- {
- pt.x += p3.x * w3;
- pt.y += p3.y * w3;
- pt.z += p3.z * w3;
- weight += w3;
- }
- if (isFinite (p4))
- {
- pt.x += p4.x * w4;
- pt.y += p4.y * w4;
- pt.z += p4.z * w4;
- weight += w4;
- }
+ /////////////////////////////////////////////////////////////////////////
+ void
+ run()
+ {
+ grabber_.start();
- if (weight == 0)
- pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
- else
- {
- weight = 1.0f / weight;
- pt.x *= weight; pt.y *= weight; pt.z *= weight;
- }
-
- return (pt);
- }
+ bool image_init = false, cloud_init = false;
+ bool keypts = true;
- /////////////////////////////////////////////////////////////////////////
- void
- get3DKeypoints (
- const CloudConstPtr &cloud,
- const PointCloud<KeyPointT>::Ptr &keypoints, PointCloud<PointT> &keypoints3d)
- {
- keypoints3d.resize (keypoints->size ());
- keypoints3d.width = keypoints->width;
- keypoints3d.height = keypoints->height;
- keypoints3d.is_dense = true;
-
- std::size_t j = 0;
- for (std::size_t i = 0; i < keypoints->size (); ++i)
- {
- PointT pt = bilinearInterpolation (cloud, keypoints->points[i].x, keypoints->points[i].y);
-
- keypoints3d.points[j].x = pt.x;
- keypoints3d.points[j].y = pt.y;
- keypoints3d.points[j].z = pt.z;
- ++j;
- }
+ PointCloud<KeyPointT>::Ptr keypoints;
+ CloudConstPtr cloud;
+ CloudPtr keypoints3d(new Cloud);
- if (j != keypoints->size ())
- {
- keypoints3d.resize (j);
- keypoints3d.width = j;
- keypoints3d.height = 1;
- }
- }
-
- /////////////////////////////////////////////////////////////////////////
- void
- run ()
- {
- grabber_.start ();
-
- bool image_init = false, cloud_init = false;
- bool keypts = true;
-
- PointCloud<KeyPointT>::Ptr keypoints;
- CloudConstPtr cloud;
- CloudPtr keypoints3d (new Cloud);
-
- while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ())
- {
- if (cloud_mutex_.try_lock ())
- {
- if (cloud_)
- cloud_.swap (cloud);
-
- if (keypoints_)
- keypoints_.swap (keypoints);
-
- cloud_mutex_.unlock ();
-
- if (!cloud)
- continue;
-
- if (!cloud_init)
- {
- cloud_viewer_.setPosition (0, 0);
- cloud_viewer_.setSize (cloud->width, cloud->height);
- cloud_init = true;
- }
-
- if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud"))
- {
- cloud_viewer_.addPointCloud (cloud, "OpenNICloud");
- cloud_viewer_.resetCameraViewpoint ("OpenNICloud");
- }
-
- if (!image_init)
- {
- image_viewer_.setPosition (cloud->width, 0);
- image_viewer_.setSize (cloud->width, cloud->height);
- image_init = true;
- }
-
- image_viewer_.showRGBImage<PointT> (cloud);
-
- image_viewer_.removeLayer (getStrBool (keypts));
- for (std::size_t i = 0; i < keypoints->size (); ++i)
- {
- int u = int (keypoints->points[i].x);
- int v = int (keypoints->points[i].y);
- image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 10, getStrBool (!keypts));
- }
- keypts = !keypts;
-
-
- get3DKeypoints (cloud, keypoints, *keypoints3d);
- visualization::PointCloudColorHandlerCustom<PointT> blue (keypoints3d, 0, 0, 255);
- if (!cloud_viewer_.updatePointCloud (keypoints3d, blue, "keypoints"))
- cloud_viewer_.addPointCloud (keypoints3d, blue, "keypoints");
- cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 10, "keypoints");
- cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
+ while (!cloud_viewer_.wasStopped() && !image_viewer_.wasStopped()) {
+ if (cloud_mutex_.try_lock()) {
+ if (cloud_)
+ cloud_.swap(cloud);
+
+ if (keypoints_)
+ keypoints_.swap(keypoints);
+
+ cloud_mutex_.unlock();
+
+ if (!cloud)
+ continue;
+
+ if (!cloud_init) {
+ cloud_viewer_.setPosition(0, 0);
+ cloud_viewer_.setSize(cloud->width, cloud->height);
+ cloud_init = true;
+ }
+
+ if (!cloud_viewer_.updatePointCloud(cloud, "OpenNICloud")) {
+ cloud_viewer_.addPointCloud(cloud, "OpenNICloud");
+ cloud_viewer_.resetCameraViewpoint("OpenNICloud");
+ }
+
+ if (!image_init) {
+ image_viewer_.setPosition(cloud->width, 0);
+ image_viewer_.setSize(cloud->width, cloud->height);
+ image_init = true;
}
- cloud_viewer_.spinOnce ();
- image_viewer_.spinOnce ();
- std::this_thread::sleep_for(100us);
+ image_viewer_.showRGBImage<PointT>(cloud);
+
+ image_viewer_.removeLayer(getStrBool(keypts));
+ for (std::size_t i = 0; i < keypoints->size(); ++i) {
+ int u = int(keypoints->points[i].x);
+ int v = int(keypoints->points[i].y);
+ image_viewer_.markPoint(u,
+ v,
+ visualization::red_color,
+ visualization::blue_color,
+ 10,
+ getStrBool(!keypts));
+ }
+ keypts = !keypts;
+
+ get3DKeypoints(cloud, keypoints, *keypoints3d);
+ visualization::PointCloudColorHandlerCustom<PointT> blue(
+ keypoints3d, 0, 0, 255);
+ if (!cloud_viewer_.updatePointCloud(keypoints3d, blue, "keypoints"))
+ cloud_viewer_.addPointCloud(keypoints3d, blue, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties(
+ visualization::PCL_VISUALIZER_POINT_SIZE, 10, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties(
+ visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
}
- grabber_.stop ();
-
- cloud_connection.disconnect ();
+ cloud_viewer_.spinOnce();
+ image_viewer_.spinOnce();
+ std::this_thread::sleep_for(100us);
}
-
- visualization::PCLVisualizer cloud_viewer_;
- Grabber& grabber_;
- std::mutex cloud_mutex_;
- CloudConstPtr cloud_;
-
- visualization::ImageViewer image_viewer_;
-
- PointCloud<KeyPointT>::Ptr keypoints_;
-
- private:
- boost::signals2::connection cloud_connection;
+
+ grabber_.stop();
+
+ cloud_connection.disconnect();
+ }
+
+ visualization::PCLVisualizer cloud_viewer_;
+ Grabber& grabber_;
+ std::mutex cloud_mutex_;
+ CloudConstPtr cloud_;
+
+ visualization::ImageViewer image_viewer_;
+
+ PointCloud<KeyPointT>::Ptr keypoints_;
+
+private:
+ boost::signals2::connection cloud_connection;
};
/* ---[ */
int
-main (int, char**)
+main(int, char**)
{
- string device_id ("#1");
- OpenNIGrabber grabber (device_id);
- BRISKDemo openni_viewer (grabber);
-
- openni_viewer.init ();
- openni_viewer.run ();
-
- return (0);
+ string device_id("#1");
+ OpenNIGrabber grabber(device_id);
+ BRISKDemo openni_viewer(grabber);
+
+ openni_viewer.init();
+ openni_viewer.run();
+
+ return 0;
}
/* ]--- */
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
*/
#include <pcl/apps/timer.h>
-#include <pcl/common/common.h>
#include <pcl/common/angles.h>
+#include <pcl/common/common.h>
#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/geometry/polygon_operations.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
+#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/search/organized.h>
-#include <pcl/features/integral_image_normal.h>
-#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/edge_aware_plane_comparator.h>
+#include <pcl/segmentation/euclidean_cluster_comparator.h>
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
+#include <pcl/segmentation/organized_connected_component_segmentation.h>
#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/segmentation/extract_polygonal_prism_data.h>
-#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/surface/convex_hull.h>
-#include <pcl/visualization/point_cloud_handlers.h>
-#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
-#include <pcl/segmentation/euclidean_cluster_comparator.h>
-#include <pcl/segmentation/organized_connected_component_segmentation.h>
-#include <pcl/segmentation/edge_aware_plane_comparator.h>
-#include <pcl/geometry/polygon_operations.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/point_cloud_handlers.h>
#include <mutex>
#include <thread>
#define SHOW_FPS 1
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-class NILinemod
-{
- public:
- using Cloud = PointCloud<PointT>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
- bool added;
-
- NILinemod (Grabber& grabber)
- : cloud_viewer_ ("PointCloud")
- , grabber_ (grabber)
- , image_viewer_ ("Image")
- , first_frame_ (true)
- {
- added = false;
-
- // Set the parameters for normal estimation
- ne_.setNormalEstimationMethod (ne_.COVARIANCE_MATRIX);
- ne_.setMaxDepthChangeFactor (0.02f);
- ne_.setNormalSmoothingSize (20.0f);
-
- // Set the parameters for planar segmentation
- plane_comparator_.reset (new EdgeAwarePlaneComparator<PointT, Normal>);
- plane_comparator_->setDistanceThreshold (0.01f, false);
- mps_.setMinInliers (5000);
- mps_.setAngularThreshold (pcl::deg2rad (3.0)); // 3 degrees
- mps_.setDistanceThreshold (0.02); // 2 cm
- mps_.setMaximumCurvature (0.001); // a small curvature
- mps_.setProjectPoints (true);
- mps_.setComparator (plane_comparator_);
- }
-
- /////////////////////////////////////////////////////////////////////////
- void
- cloud_callback (const CloudConstPtr& cloud)
- {
- FPS_CALC ("cloud callback");
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- cloud_ = cloud;
- search_.setInputCloud (cloud);
-
- // Subsequent frames are segmented by "tracking" the parameters of the previous frame
- // We do this by using the estimated inliers from previous frames in the current frame,
- // and refining the coefficients
-
- if (!first_frame_)
- {
- if (!plane_indices_ || plane_indices_->indices.empty () || !search_.getInputCloud ())
- {
- PCL_ERROR ("Lost tracking. Select the object again to continue.\n");
- first_frame_ = true;
- return;
- }
- SACSegmentation<PointT> seg;
- seg.setOptimizeCoefficients (true);
- seg.setModelType (SACMODEL_PLANE);
- seg.setMethodType (SAC_RANSAC);
- seg.setMaxIterations (1000);
- seg.setDistanceThreshold (0.01);
- seg.setInputCloud (search_.getInputCloud ());
- seg.setIndices (plane_indices_);
- ModelCoefficients coefficients;
- PointIndices inliers;
- seg.segment (inliers, coefficients);
-
- if (inliers.indices.empty ())
- {
- PCL_ERROR ("No planar model found. Select the object again to continue.\n");
- first_frame_ = true;
- return;
- }
+class NILinemod {
+public:
+ using Cloud = PointCloud<PointT>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+ bool added;
+
+ NILinemod(Grabber& grabber)
+ : cloud_viewer_("PointCloud")
+ , grabber_(grabber)
+ , image_viewer_("Image")
+ , first_frame_(true)
+ {
+ added = false;
+
+ // Set the parameters for normal estimation
+ ne_.setNormalEstimationMethod(ne_.COVARIANCE_MATRIX);
+ ne_.setMaxDepthChangeFactor(0.02f);
+ ne_.setNormalSmoothingSize(20.0f);
+
+ // Set the parameters for planar segmentation
+ plane_comparator_.reset(new EdgeAwarePlaneComparator<PointT, Normal>);
+ plane_comparator_->setDistanceThreshold(0.01f, false);
+ mps_.setMinInliers(5000);
+ mps_.setAngularThreshold(pcl::deg2rad(3.0)); // 3 degrees
+ mps_.setDistanceThreshold(0.02); // 2 cm
+ mps_.setMaximumCurvature(0.001); // a small curvature
+ mps_.setProjectPoints(true);
+ mps_.setComparator(plane_comparator_);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ cloud_callback(const CloudConstPtr& cloud)
+ {
+ FPS_CALC("cloud callback");
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ cloud_ = cloud;
+ search_.setInputCloud(cloud);
+
+ // Subsequent frames are segmented by "tracking" the parameters of the previous
+ // frame We do this by using the estimated inliers from previous frames in the
+ // current frame, and refining the coefficients
+
+ if (!first_frame_) {
+ if (!plane_indices_ || plane_indices_->indices.empty() ||
+ !search_.getInputCloud()) {
+ PCL_ERROR("Lost tracking. Select the object again to continue.\n");
+ first_frame_ = true;
+ return;
+ }
+ SACSegmentation<PointT> seg;
+ seg.setOptimizeCoefficients(true);
+ seg.setModelType(SACMODEL_PLANE);
+ seg.setMethodType(SAC_RANSAC);
+ seg.setMaxIterations(1000);
+ seg.setDistanceThreshold(0.01);
+ seg.setInputCloud(search_.getInputCloud());
+ seg.setIndices(plane_indices_);
+ ModelCoefficients coefficients;
+ PointIndices inliers;
+ seg.segment(inliers, coefficients);
+
+ if (inliers.indices.empty()) {
+ PCL_ERROR("No planar model found. Select the object again to continue.\n");
+ first_frame_ = true;
+ return;
+ }
- // Visualize the object in 3D...
- CloudPtr plane_inliers (new Cloud);
- pcl::copyPointCloud (*search_.getInputCloud (), inliers.indices, *plane_inliers);
- if (plane_inliers->points.empty ())
- {
- PCL_ERROR ("No planar model found. Select the object again to continue.\n");
- first_frame_ = true;
- return;
- }
- plane_.reset (new Cloud);
+ // Visualize the object in 3D...
+ CloudPtr plane_inliers(new Cloud);
+ pcl::copyPointCloud(*search_.getInputCloud(), inliers.indices, *plane_inliers);
+ if (plane_inliers->points.empty()) {
+ PCL_ERROR("No planar model found. Select the object again to continue.\n");
+ first_frame_ = true;
+ return;
+ }
+ plane_.reset(new Cloud);
- // Compute the convex hull of the plane
- ConvexHull<PointT> chull;
- chull.setDimension (2);
- chull.setInputCloud (plane_inliers);
- chull.reconstruct (*plane_);
+ // Compute the convex hull of the plane
+ ConvexHull<PointT> chull;
+ chull.setDimension(2);
+ chull.setInputCloud(plane_inliers);
+ chull.reconstruct(*plane_);
+ }
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ CloudConstPtr
+ getLatestCloud()
+ {
+ // Lock while we swap our cloud and reset it.
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ CloudConstPtr temp_cloud;
+ temp_cloud.swap(cloud_);
+ return temp_cloud;
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ keyboard_callback(const visualization::KeyboardEvent&, void*)
+ {
+ // if (event.getKeyCode())
+ // std::cout << "the key \'" << event.getKeyCode() << "\' (" << event.getKeyCode()
+ // << ") was";
+ // else
+ // std::cout << "the special key \'" << event.getKeySym() << "\' was";
+ // if (event.keyDown())
+ // std::cout << " pressed" << std::endl;
+ // else
+ // std::cout << " released" << std::endl;
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ mouse_callback(const visualization::MouseEvent&, void*)
+ {
+ // if (mouse_event.getType() == visualization::MouseEvent::MouseButtonPress &&
+ // mouse_event.getButton() == visualization::MouseEvent::LeftButton)
+ //{
+ // std::cout << "left button pressed @ " << mouse_event.getX () << " , " <<
+ // mouse_event.getY () << std::endl;
+ //}
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ /**
+ * \brief Given a plane, and the set of inlier indices representing it,
+ * segment out the object of intererest supported by it.
+ *
+ * \param[in] picked_idx the index of a point on the object
+ * \param[in] cloud the full point cloud dataset
+ * \param[in] plane_indices a set of indices representing the plane supporting the
+ * object of interest
+ * \param[in] plane_boundary_indices a set of indices representing
+ * the boundary of the plane
+ * \param[out] object the segmented resultant object
+ */
+ void
+ segmentObject(int picked_idx,
+ const CloudConstPtr& cloud,
+ const PointIndices::Ptr& plane_indices,
+ const PointIndices::Ptr& plane_boundary_indices,
+ Cloud& object)
+ {
+ CloudPtr plane_hull(new Cloud);
+
+ // Compute the convex hull of the plane
+ ConvexHull<PointT> chull;
+ chull.setDimension(2);
+ chull.setInputCloud(cloud);
+ chull.setIndices(plane_boundary_indices);
+ chull.reconstruct(*plane_hull);
+
+ // Remove the plane indices from the data
+ PointIndices::Ptr everything_but_the_plane(new PointIndices);
+ if (indices_fullset_.size() != cloud->points.size()) {
+ indices_fullset_.resize(cloud->points.size());
+ for (int p_it = 0; p_it < static_cast<int>(indices_fullset_.size()); ++p_it)
+ indices_fullset_[p_it] = p_it;
+ }
+ std::vector<int> indices_subset = plane_indices->indices;
+ std::sort(indices_subset.begin(), indices_subset.end());
+ set_difference(indices_fullset_.begin(),
+ indices_fullset_.end(),
+ indices_subset.begin(),
+ indices_subset.end(),
+ inserter(everything_but_the_plane->indices,
+ everything_but_the_plane->indices.begin()));
+
+ // Extract all clusters above the hull
+ PointIndices::Ptr points_above_plane(new PointIndices);
+ ExtractPolygonalPrismData<PointT> exppd;
+ exppd.setInputCloud(cloud);
+ exppd.setInputPlanarHull(plane_hull);
+ exppd.setIndices(everything_but_the_plane);
+ exppd.setHeightLimits(0.0, 0.5); // up to half a meter
+ exppd.segment(*points_above_plane);
+
+ // Use an organized clustering segmentation to extract the individual clusters
+ EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator(
+ new EuclideanClusterComparator<PointT, Label>);
+ euclidean_cluster_comparator->setInputCloud(cloud);
+ euclidean_cluster_comparator->setDistanceThreshold(0.03f, false);
+ // Set the entire scene to false, and the inliers of the objects located on top of
+ // the plane to true
+ Label l;
+ l.label = 0;
+ PointCloud<Label>::Ptr scene(new PointCloud<Label>(cloud->width, cloud->height, l));
+ // Mask the objects that we want to split into clusters
+ for (const int& index : points_above_plane->indices)
+ scene->points[index].label = 1;
+ euclidean_cluster_comparator->setLabels(scene);
+
+ OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation(
+ euclidean_cluster_comparator);
+ euclidean_segmentation.setInputCloud(cloud);
+
+ PointCloud<Label> euclidean_labels;
+ std::vector<PointIndices> euclidean_label_indices;
+ euclidean_segmentation.segment(euclidean_labels, euclidean_label_indices);
+
+ // For each cluster found
+ bool cluster_found = false;
+ for (const auto& euclidean_label_index : euclidean_label_indices) {
+ if (cluster_found)
+ break;
+ // Check if the point that we picked belongs to it
+ for (std::size_t j = 0; j < euclidean_label_index.indices.size(); ++j) {
+ if (picked_idx != euclidean_label_index.indices[j])
+ continue;
+ // pcl::PointCloud<PointT> cluster;
+ pcl::copyPointCloud(*cloud, euclidean_label_index.indices, object);
+ cluster_found = true;
+ break;
+ // object_indices = euclidean_label_indices[i].indices;
+ // clusters.push_back (cluster);
+ }
+ }
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ segment(const PointT& picked_point,
+ int picked_idx,
+ PlanarRegion<PointT>& region,
+ PointIndices&,
+ CloudPtr& object)
+ {
+ // First frame is segmented using an organized multi plane segmentation approach
+ // from points and their normals
+ if (!first_frame_)
+ return;
+
+ // Estimate normals in the cloud
+ PointCloud<Normal>::Ptr normal_cloud(new PointCloud<Normal>);
+ ne_.setInputCloud(search_.getInputCloud());
+ ne_.compute(*normal_cloud);
+
+ plane_comparator_->setDistanceMap(ne_.getDistanceMap());
+
+ // Segment out all planes
+ mps_.setInputNormals(normal_cloud);
+ mps_.setInputCloud(search_.getInputCloud());
+
+ // Use one of the overloaded segmentAndRefine calls to get all the information that
+ // we want out
+ std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT>>>
+ regions;
+ std::vector<ModelCoefficients> model_coefficients;
+ std::vector<PointIndices> inlier_indices;
+ PointCloud<Label>::Ptr labels(new PointCloud<Label>);
+ std::vector<PointIndices> label_indices;
+ std::vector<PointIndices> boundary_indices;
+ mps_.segmentAndRefine(regions,
+ model_coefficients,
+ inlier_indices,
+ labels,
+ label_indices,
+ boundary_indices);
+ PCL_DEBUG("Number of planar regions detected: %lu for a cloud of %lu points and "
+ "%lu normals.\n",
+ regions.size(),
+ search_.getInputCloud()->points.size(),
+ normal_cloud->points.size());
+
+ double max_dist = std::numeric_limits<double>::max();
+ // Compute the distances from all the planar regions to the picked point, and select
+ // the closest region
+ int idx = -1;
+ for (std::size_t i = 0; i < regions.size(); ++i) {
+ double dist = pointToPlaneDistance(picked_point, regions[i].getCoefficients());
+ if (dist < max_dist) {
+ max_dist = dist;
+ idx = static_cast<int>(i);
}
}
- /////////////////////////////////////////////////////////////////////////
- CloudConstPtr
- getLatestCloud ()
- {
- // Lock while we swap our cloud and reset it.
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- CloudConstPtr temp_cloud;
- temp_cloud.swap (cloud_);
- return (temp_cloud);
+ PointIndices::Ptr plane_boundary_indices;
+ // Get the plane that holds the object of interest
+ if (idx != -1) {
+ region = regions[idx];
+ plane_indices_.reset(new PointIndices(inlier_indices[idx]));
+ plane_boundary_indices.reset(new PointIndices(boundary_indices[idx]));
}
- /////////////////////////////////////////////////////////////////////////
- void
- keyboard_callback (const visualization::KeyboardEvent&, void*)
- {
- //if (event.getKeyCode())
- // std::cout << "the key \'" << event.getKeyCode() << "\' (" << event.getKeyCode() << ") was";
- //else
- // std::cout << "the special key \'" << event.getKeySym() << "\' was";
- //if (event.keyDown())
- // std::cout << " pressed" << std::endl;
- //else
- // std::cout << " released" << std::endl;
+ // Segment the object of interest
+ if (plane_boundary_indices && !plane_boundary_indices->indices.empty()) {
+ object.reset(new Cloud);
+ segmentObject(picked_idx,
+ search_.getInputCloud(),
+ plane_indices_,
+ plane_boundary_indices,
+ *object);
+
+ // Save to disk
+ // pcl::io::saveTARPointCloud ("output.ltm", *object, "object.pcd");
}
-
- /////////////////////////////////////////////////////////////////////////
- void
- mouse_callback (const visualization::MouseEvent&, void*)
- {
- //if (mouse_event.getType() == visualization::MouseEvent::MouseButtonPress && mouse_event.getButton() == visualization::MouseEvent::LeftButton)
- //{
- // std::cout << "left button pressed @ " << mouse_event.getX () << " , " << mouse_event.getY () << std::endl;
- //}
+ first_frame_ = false;
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ /**
+ * \brief Point picking callback. This gets called when the user selects
+ * a 3D point on screen (in the PCLVisualizer window) using Shift+click.
+ *
+ * \param[in] event the event that triggered the call
+ */
+ void
+ pp_callback(const visualization::PointPickingEvent& event, void*)
+ {
+ // Check to see if we got a valid point. Early exit.
+ int idx = event.getPointIndex();
+ if (idx == -1)
+ return;
+
+ std::vector<int> indices(1);
+ std::vector<float> distances(1);
+
+ // Use mutices to make sure we get the right cloud
+ std::lock_guard<std::mutex> lock1(cloud_mutex_);
+
+ // Get the point that was picked
+ PointT picked_pt;
+ event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z);
+
+ // Add a sphere to it in the PCLVisualizer window
+ stringstream ss;
+ ss << "sphere_" << idx;
+ cloud_viewer_.addSphere(picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str());
+
+ // Check to see if we have access to the actual cloud data. Use the previously built
+ // search object.
+ if (!search_.isValid())
+ return;
+
+ // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we
+ // must search for the real point
+ search_.nearestKSearch(picked_pt, 1, indices, distances);
+
+ // Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is
+ // bottom left.
+ std::uint32_t width = search_.getInputCloud()->width;
+ // height = search_.getInputCloud ()->height;
+ int v = indices[0] / width, u = indices[0] % width;
+
+ // Add some marker to the image
+ image_viewer_.addCircle(u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
+ image_viewer_.addFilledRectangle(
+ u - 5, u + 5, v - 5, v + 5, 0.0, 1.0, 0.0, "boxes", 0.5);
+ image_viewer_.markPoint(
+ u, v, visualization::red_color, visualization::blue_color, 10);
+
+ // Segment the region that we're interested in, by employing a two step process:
+ // * first, segment all the planes in the scene, and find the one closest to our
+ // picked point
+ // * then, use euclidean clustering to find the object that we clicked on and
+ // return it
+ PlanarRegion<PointT> region;
+ CloudPtr object;
+ PointIndices region_indices;
+ segment(picked_pt, indices[0], region, region_indices, object);
+
+ // If no region could be determined, exit
+ if (region.getContour().empty()) {
+ PCL_ERROR("No planar region detected. Please select another point or relax the "
+ "thresholds and continue.\n");
+ return;
}
-
- /////////////////////////////////////////////////////////////////////////
- /** \brief Given a plane, and the set of inlier indices representing it,
- * segment out the object of intererest supported by it.
- *
- * \param[in] picked_idx the index of a point on the object
- * \param[in] cloud the full point cloud dataset
- * \param[in] plane_indices a set of indices representing the plane supporting the object of interest
- * \param[in] plane_boundary_indices a set of indices representing the boundary of the plane
- * \param[out] object the segmented resultant object
- */
- void
- segmentObject (int picked_idx,
- const CloudConstPtr &cloud,
- const PointIndices::Ptr &plane_indices,
- const PointIndices::Ptr &plane_boundary_indices,
- Cloud &object)
- {
- CloudPtr plane_hull (new Cloud);
-
- // Compute the convex hull of the plane
- ConvexHull<PointT> chull;
- chull.setDimension (2);
- chull.setInputCloud (cloud);
- chull.setIndices (plane_boundary_indices);
- chull.reconstruct (*plane_hull);
-
- // Remove the plane indices from the data
- PointIndices::Ptr everything_but_the_plane (new PointIndices);
- if (indices_fullset_.size () != cloud->points.size ())
- {
- indices_fullset_.resize (cloud->points.size ());
- for (int p_it = 0; p_it < static_cast<int> (indices_fullset_.size ()); ++p_it)
- indices_fullset_[p_it] = p_it;
- }
- std::vector<int> indices_subset = plane_indices->indices;
- std::sort (indices_subset.begin (), indices_subset.end ());
- set_difference (indices_fullset_.begin (), indices_fullset_.end (),
- indices_subset.begin (), indices_subset.end (),
- inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
-
- // Extract all clusters above the hull
- PointIndices::Ptr points_above_plane (new PointIndices);
- ExtractPolygonalPrismData<PointT> exppd;
- exppd.setInputCloud (cloud);
- exppd.setInputPlanarHull (plane_hull);
- exppd.setIndices (everything_but_the_plane);
- exppd.setHeightLimits (0.0, 0.5); // up to half a meter
- exppd.segment (*points_above_plane);
-
- // Use an organized clustering segmentation to extract the individual clusters
- EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
- euclidean_cluster_comparator->setInputCloud (cloud);
- euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
- // Set the entire scene to false, and the inliers of the objects located on top of the plane to true
- Label l; l.label = 0;
- PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
- // Mask the objects that we want to split into clusters
- for (const int &index : points_above_plane->indices)
- scene->points[index].label = 1;
- euclidean_cluster_comparator->setLabels (scene);
-
- OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
- euclidean_segmentation.setInputCloud (cloud);
-
- PointCloud<Label> euclidean_labels;
- std::vector<PointIndices> euclidean_label_indices;
- euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
-
- // For each cluster found
- bool cluster_found = false;
- for (const auto &euclidean_label_index : euclidean_label_indices)
- {
- if (cluster_found)
- break;
- // Check if the point that we picked belongs to it
- for (std::size_t j = 0; j < euclidean_label_index.indices.size (); ++j)
- {
- if (picked_idx != euclidean_label_index.indices[j])
- continue;
- //pcl::PointCloud<PointT> cluster;
- pcl::copyPointCloud (*cloud, euclidean_label_index.indices, object);
- cluster_found = true;
- break;
- //object_indices = euclidean_label_indices[i].indices;
- //clusters.push_back (cluster);
- }
- }
+ // Else, draw it on screen
+ // cloud_viewer_.addPolygon (region, 1.0, 0.0, 0.0, "region");
+ // cloud_viewer_.setShapeRenderingProperties
+ // (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");
+
+ PlanarRegion<PointT> refined_region;
+ pcl::approximatePolygon(region, refined_region, 0.01, false, true);
+ PCL_INFO("Planar region: %lu points initial, %lu points after refinement.\n",
+ region.getContour().size(),
+ refined_region.getContour().size());
+ cloud_viewer_.addPolygon(refined_region, 0.0, 0.0, 1.0, "refined_region");
+ cloud_viewer_.setShapeRenderingProperties(
+ visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "refined_region");
+
+ // Draw in image space
+ image_viewer_.addPlanarPolygon(
+ search_.getInputCloud(), refined_region, 0.0, 0.0, 1.0, "refined_region", 1.0);
+
+ // If no object could be determined, exit
+ if (!object) {
+ PCL_ERROR("No object detected. Please select another point or relax the "
+ "thresholds and continue.\n");
+ return;
}
+ // Visualize the object in 3D...
+ visualization::PointCloudColorHandlerCustom<PointT> red(object, 255, 0, 0);
+ if (!cloud_viewer_.updatePointCloud(object, red, "object"))
+ cloud_viewer_.addPointCloud(object, red, "object");
+ // ...and 2D
+ image_viewer_.removeLayer("object");
+ image_viewer_.addMask(search_.getInputCloud(), *object, "object");
+
+ // Compute the min/max of the object
+ PointT min_pt, max_pt;
+ getMinMax3D(*object, min_pt, max_pt);
+ stringstream ss2;
+ ss2 << "cube_" << idx;
+ // Visualize the bounding box in 3D...
+ cloud_viewer_.addCube(min_pt.x,
+ max_pt.x,
+ min_pt.y,
+ max_pt.y,
+ min_pt.z,
+ max_pt.z,
+ 0.0,
+ 1.0,
+ 0.0,
+ ss2.str());
+ cloud_viewer_.setShapeRenderingProperties(
+ visualization::PCL_VISUALIZER_LINE_WIDTH, 10, ss2.str());
+
+ // ...and 2D
+ image_viewer_.addRectangle(search_.getInputCloud(), *object);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ init()
+ {
+ cloud_viewer_.registerMouseCallback(&NILinemod::mouse_callback, *this);
+ cloud_viewer_.registerKeyboardCallback(&NILinemod::keyboard_callback, *this);
+ cloud_viewer_.registerPointPickingCallback(&NILinemod::pp_callback, *this);
+ std::function<void(const CloudConstPtr&)> cloud_cb =
+ [this](const CloudConstPtr& cloud) { cloud_callback(cloud); };
+ cloud_connection = grabber_.registerCallback(cloud_cb);
+
+ image_viewer_.registerMouseCallback(&NILinemod::mouse_callback, *this);
+ image_viewer_.registerKeyboardCallback(&NILinemod::keyboard_callback, *this);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ run()
+ {
+ grabber_.start();
+
+ bool image_init = false, cloud_init = false;
+
+ while (!cloud_viewer_.wasStopped() && !image_viewer_.wasStopped()) {
+ if (cloud_) {
+ CloudConstPtr cloud = getLatestCloud();
+ if (!cloud_init) {
+ cloud_viewer_.setPosition(0, 0);
+ cloud_viewer_.setSize(cloud->width, cloud->height);
+ cloud_init = true;
+ }
+ if (!cloud_viewer_.updatePointCloud(cloud, "OpenNICloud")) {
+ cloud_viewer_.addPointCloud(cloud, "OpenNICloud");
+ cloud_viewer_.resetCameraViewpoint("OpenNICloud");
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- segment (const PointT &picked_point,
- int picked_idx,
- PlanarRegion<PointT> ®ion,
- PointIndices &,
- CloudPtr &object)
- {
- // First frame is segmented using an organized multi plane segmentation approach from points and their normals
- if (!first_frame_)
- return;
-
- // Estimate normals in the cloud
- PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
- ne_.setInputCloud (search_.getInputCloud ());
- ne_.compute (*normal_cloud);
-
- plane_comparator_->setDistanceMap (ne_.getDistanceMap ());
-
- // Segment out all planes
- mps_.setInputNormals (normal_cloud);
- mps_.setInputCloud (search_.getInputCloud ());
-
- // Use one of the overloaded segmentAndRefine calls to get all the information that we want out
- std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > > regions;
- std::vector<ModelCoefficients> model_coefficients;
- std::vector<PointIndices> inlier_indices;
- PointCloud<Label>::Ptr labels (new PointCloud<Label>);
- std::vector<PointIndices> label_indices;
- std::vector<PointIndices> boundary_indices;
- mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
- PCL_DEBUG ("Number of planar regions detected: %lu for a cloud of %lu points and %lu normals.\n", regions.size (), search_.getInputCloud ()->points.size (), normal_cloud->points.size ());
-
- double max_dist = std::numeric_limits<double>::max ();
- // Compute the distances from all the planar regions to the picked point, and select the closest region
- int idx = -1;
- for (std::size_t i = 0; i < regions.size (); ++i)
- {
- double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ());
- if (dist < max_dist)
- {
- max_dist = dist;
- idx = static_cast<int> (i);
+ if (!image_init) {
+ image_viewer_.setPosition(cloud->width, 0);
+ image_viewer_.setSize(cloud->width, cloud->height);
+ image_init = true;
}
- }
- PointIndices::Ptr plane_boundary_indices;
- // Get the plane that holds the object of interest
- if (idx != -1)
- {
- region = regions[idx];
- plane_indices_.reset (new PointIndices (inlier_indices[idx]));
- plane_boundary_indices.reset (new PointIndices (boundary_indices[idx]));
+ image_viewer_.showRGBImage<PointT>(cloud);
}
- // Segment the object of interest
- if (plane_boundary_indices && !plane_boundary_indices->indices.empty ())
- {
- object.reset (new Cloud);
- segmentObject (picked_idx, search_.getInputCloud (), plane_indices_, plane_boundary_indices, *object);
-
- // Save to disk
- //pcl::io::saveTARPointCloud ("output.ltm", *object, "object.pcd");
- }
- first_frame_ = false;
+ // Add the plane that we're tracking to the cloud visualizer
+ CloudPtr plane(new Cloud);
+ if (plane_)
+ *plane = *plane_;
+ visualization::PointCloudColorHandlerCustom<PointT> blue(plane, 0, 255, 0);
+ if (!cloud_viewer_.updatePointCloud(plane, blue, "plane"))
+ cloud_viewer_.addPointCloud(plane, "plane");
+ cloud_viewer_.spinOnce();
+
+ image_viewer_.spinOnce();
+ std::this_thread::sleep_for(100us);
}
- /////////////////////////////////////////////////////////////////////////
- /** \brief Point picking callback. This gets called when the user selects
- * a 3D point on screen (in the PCLVisualizer window) using Shift+click.
- *
- * \param[in] event the event that triggered the call
- */
- void
- pp_callback (const visualization::PointPickingEvent& event, void*)
- {
- // Check to see if we got a valid point. Early exit.
- int idx = event.getPointIndex ();
- if (idx == -1)
- return;
-
- std::vector<int> indices (1);
- std::vector<float> distances (1);
+ grabber_.stop();
- // Use mutices to make sure we get the right cloud
- std::lock_guard<std::mutex> lock1 (cloud_mutex_);
+ cloud_connection.disconnect();
+ }
- // Get the point that was picked
- PointT picked_pt;
- event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
+ visualization::PCLVisualizer cloud_viewer_;
+ Grabber& grabber_;
+ std::mutex cloud_mutex_;
+ CloudConstPtr cloud_;
- // Add a sphere to it in the PCLVisualizer window
- stringstream ss;
- ss << "sphere_" << idx;
- cloud_viewer_.addSphere (picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str ());
+ visualization::ImageViewer image_viewer_;
- // Check to see if we have access to the actual cloud data. Use the previously built search object.
- if (!search_.isValid ())
- return;
+ search::OrganizedNeighbor<PointT> search_;
- // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
- search_.nearestKSearch (picked_pt, 1, indices, distances);
-
- // Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is bottom left.
- std::uint32_t width = search_.getInputCloud ()->width;
-// height = search_.getInputCloud ()->height;
- int v = indices[0] / width,
- u = indices[0] % width;
-
- // Add some marker to the image
- image_viewer_.addCircle (u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
- image_viewer_.addFilledRectangle (u-5, u+5, v-5, v+5, 0.0, 1.0, 0.0, "boxes", 0.5);
- image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 10);
-
- // Segment the region that we're interested in, by employing a two step process:
- // * first, segment all the planes in the scene, and find the one closest to our picked point
- // * then, use euclidean clustering to find the object that we clicked on and return it
- PlanarRegion<PointT> region;
- CloudPtr object;
- PointIndices region_indices;
- segment (picked_pt, indices[0], region, region_indices, object);
-
- // If no region could be determined, exit
- if (region.getContour ().empty ())
- {
- PCL_ERROR ("No planar region detected. Please select another point or relax the thresholds and continue.\n");
- return;
- }
- // Else, draw it on screen
- //cloud_viewer_.addPolygon (region, 1.0, 0.0, 0.0, "region");
- //cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");
-
- PlanarRegion<PointT> refined_region;
- pcl::approximatePolygon (region, refined_region, 0.01, false, true);
- PCL_INFO ("Planar region: %lu points initial, %lu points after refinement.\n", region.getContour ().size (), refined_region.getContour ().size ());
- cloud_viewer_.addPolygon (refined_region, 0.0, 0.0, 1.0, "refined_region");
- cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "refined_region");
-
- // Draw in image space
- image_viewer_.addPlanarPolygon (search_.getInputCloud (), refined_region, 0.0, 0.0, 1.0, "refined_region", 1.0);
-
- // If no object could be determined, exit
- if (!object)
- {
- PCL_ERROR ("No object detected. Please select another point or relax the thresholds and continue.\n");
- return;
- }
- // Visualize the object in 3D...
- visualization::PointCloudColorHandlerCustom<PointT> red (object, 255, 0, 0);
- if (!cloud_viewer_.updatePointCloud (object, red, "object"))
- cloud_viewer_.addPointCloud (object, red, "object");
- // ...and 2D
- image_viewer_.removeLayer ("object");
- image_viewer_.addMask (search_.getInputCloud (), *object, "object");
-
- // Compute the min/max of the object
- PointT min_pt, max_pt;
- getMinMax3D (*object, min_pt, max_pt);
- stringstream ss2;
- ss2 << "cube_" << idx;
- // Visualize the bounding box in 3D...
- cloud_viewer_.addCube (min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z, 0.0, 1.0, 0.0, ss2.str ());
- cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, ss2.str ());
-
- // ...and 2D
- image_viewer_.addRectangle (search_.getInputCloud (), *object);
- }
-
- /////////////////////////////////////////////////////////////////////////
- void
- init ()
- {
- cloud_viewer_.registerMouseCallback (&NILinemod::mouse_callback, *this);
- cloud_viewer_.registerKeyboardCallback(&NILinemod::keyboard_callback, *this);
- cloud_viewer_.registerPointPickingCallback (&NILinemod::pp_callback, *this);
- std::function<void (const CloudConstPtr&) > cloud_cb = [this] (const CloudConstPtr& cloud) { cloud_callback (cloud); };
- cloud_connection = grabber_.registerCallback (cloud_cb);
-
- image_viewer_.registerMouseCallback (&NILinemod::mouse_callback, *this);
- image_viewer_.registerKeyboardCallback(&NILinemod::keyboard_callback, *this);
- }
+private:
+ boost::signals2::connection cloud_connection, image_connection;
+ bool first_frame_;
- /////////////////////////////////////////////////////////////////////////
- void
- run ()
- {
- grabber_.start ();
-
- bool image_init = false, cloud_init = false;
-
- while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ())
- {
- if (cloud_)
- {
- CloudConstPtr cloud = getLatestCloud ();
- if (!cloud_init)
- {
- cloud_viewer_.setPosition (0, 0);
- cloud_viewer_.setSize (cloud->width, cloud->height);
- cloud_init = true;
- }
-
- if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud"))
- {
- cloud_viewer_.addPointCloud (cloud, "OpenNICloud");
- cloud_viewer_.resetCameraViewpoint ("OpenNICloud");
- }
-
- if (!image_init)
- {
- image_viewer_.setPosition (cloud->width, 0);
- image_viewer_.setSize (cloud->width, cloud->height);
- image_init = true;
- }
-
- image_viewer_.showRGBImage<PointT> (cloud);
- }
-
-
- // Add the plane that we're tracking to the cloud visualizer
- CloudPtr plane (new Cloud);
- if (plane_)
- *plane = *plane_;
- visualization::PointCloudColorHandlerCustom<PointT> blue (plane, 0, 255, 0);
- if (!cloud_viewer_.updatePointCloud (plane, blue, "plane"))
- cloud_viewer_.addPointCloud (plane, "plane");
- cloud_viewer_.spinOnce ();
-
- image_viewer_.spinOnce ();
- std::this_thread::sleep_for(100us);
- }
-
- grabber_.stop ();
-
- cloud_connection.disconnect ();
- }
-
- visualization::PCLVisualizer cloud_viewer_;
- Grabber& grabber_;
- std::mutex cloud_mutex_;
- CloudConstPtr cloud_;
-
- visualization::ImageViewer image_viewer_;
-
- search::OrganizedNeighbor<PointT> search_;
- private:
- boost::signals2::connection cloud_connection, image_connection;
- bool first_frame_;
-
- // Segmentation
- std::vector<int> indices_fullset_;
- PointIndices::Ptr plane_indices_;
- CloudPtr plane_;
- IntegralImageNormalEstimation<PointT, Normal> ne_;
- OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps_;
- EdgeAwarePlaneComparator<PointT, Normal>::Ptr plane_comparator_;
+ // Segmentation
+ std::vector<int> indices_fullset_;
+ PointIndices::Ptr plane_indices_;
+ CloudPtr plane_;
+ IntegralImageNormalEstimation<PointT, Normal> ne_;
+ OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps_;
+ EdgeAwarePlaneComparator<PointT, Normal>::Ptr plane_comparator_;
};
/* ---[ */
int
-main (int, char**)
+main(int, char**)
{
- string device_id ("#1");
- OpenNIGrabber grabber (device_id);
- NILinemod openni_viewer (grabber);
-
- openni_viewer.init ();
- openni_viewer.run ();
-
- return (0);
+ string device_id("#1");
+ OpenNIGrabber grabber(device_id);
+ NILinemod openni_viewer(grabber);
+
+ openni_viewer.init();
+ openni_viewer.run();
+
+ return 0;
}
/* ]--- */
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
#define SHOW_FPS 1
#include <pcl/apps/timer.h>
-#include <pcl/common/common.h>
#include <pcl/common/angles.h>
+#include <pcl/common/common.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/filters/extract_indices.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/susan.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
+#include <pcl/visualization/pcl_visualizer.h>
#include <mutex>
#include <thread>
using KeyPointT = PointXYZRGBL;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-class SUSANDemo
-{
- public:
- using Cloud = PointCloud<PointT>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
-
- SUSANDemo (Grabber& grabber)
- : cloud_viewer_ ("SUSAN 2D Keypoints -- PointCloud")
- , grabber_ (grabber)
- , image_viewer_ ("SUSAN 2D Keypoints -- Image")
- {
- }
+class SUSANDemo {
+public:
+ using Cloud = PointCloud<PointT>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ SUSANDemo(Grabber& grabber)
+ : cloud_viewer_("SUSAN 2D Keypoints -- PointCloud")
+ , grabber_(grabber)
+ , image_viewer_("SUSAN 2D Keypoints -- Image")
+ {}
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ cloud_callback(const CloudConstPtr& cloud)
+ {
+ FPS_CALC("cloud callback");
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ cloud_ = cloud;
+
+ // Compute SUSAN keypoints
+ SUSANKeypoint<PointT, KeyPointT> susan;
+ susan.setInputCloud(cloud);
+ susan.setNumberOfThreads(6);
+ susan.setNonMaxSupression(true);
+ keypoints_.reset(new PointCloud<KeyPointT>);
+ susan.compute(*keypoints_);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ init()
+ {
+ std::function<void(const CloudConstPtr&)> cloud_cb =
+ [this](const CloudConstPtr& cloud) { cloud_callback(cloud); };
+ cloud_connection = grabber_.registerCallback(cloud_cb);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ string
+ getStrBool(bool state)
+ {
+ stringstream ss;
+ ss << state;
+ return ss.str();
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ run()
+ {
+ grabber_.start();
+
+ bool image_init = false, cloud_init = false;
+ bool keypts = true;
+
+ while (!cloud_viewer_.wasStopped() && !image_viewer_.wasStopped()) {
+ PointCloud<KeyPointT>::Ptr keypoints;
+ CloudConstPtr cloud;
+
+ if (cloud_mutex_.try_lock()) {
+ cloud_.swap(cloud);
+ keypoints_.swap(keypoints);
+
+ cloud_mutex_.unlock();
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- cloud_callback (const CloudConstPtr& cloud)
- {
- FPS_CALC ("cloud callback");
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- cloud_ = cloud;
-
- // Compute SUSAN keypoints
- SUSANKeypoint<PointT, KeyPointT> susan;
- susan.setInputCloud (cloud);
- susan.setNumberOfThreads (6);
- susan.setNonMaxSupression (true);
- keypoints_.reset (new PointCloud<KeyPointT>);
- susan.compute (*keypoints_);
- }
+ if (cloud) {
+ if (!cloud_init) {
+ cloud_viewer_.setPosition(0, 0);
+ cloud_viewer_.setSize(cloud->width, cloud->height);
+ cloud_init = true;
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- init ()
- {
- std::function<void (const CloudConstPtr&) > cloud_cb = [this] (const CloudConstPtr& cloud) { cloud_callback (cloud); };
- cloud_connection = grabber_.registerCallback (cloud_cb);
- }
+ if (!cloud_viewer_.updatePointCloud(cloud, "OpenNICloud")) {
+ cloud_viewer_.addPointCloud(cloud, "OpenNICloud");
+ cloud_viewer_.resetCameraViewpoint("OpenNICloud");
+ }
- /////////////////////////////////////////////////////////////////////////
- string
- getStrBool (bool state)
- {
- stringstream ss;
- ss << state;
- return (ss.str ());
- }
-
- /////////////////////////////////////////////////////////////////////////
- void
- run ()
- {
- grabber_.start ();
-
- bool image_init = false, cloud_init = false;
- bool keypts = true;
-
- while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ())
- {
- PointCloud<KeyPointT>::Ptr keypoints;
- CloudConstPtr cloud;
-
- if (cloud_mutex_.try_lock ())
- {
- cloud_.swap (cloud);
- keypoints_.swap (keypoints);
-
- cloud_mutex_.unlock ();
+ if (!image_init) {
+ image_viewer_.setPosition(cloud->width, 0);
+ image_viewer_.setSize(cloud->width, cloud->height);
+ image_init = true;
}
- if (cloud)
- {
- if (!cloud_init)
- {
- cloud_viewer_.setPosition (0, 0);
- cloud_viewer_.setSize (cloud->width, cloud->height);
- cloud_init = true;
+ image_viewer_.addRGBImage<PointT>(cloud);
+
+ if (keypoints && !keypoints->empty()) {
+ image_viewer_.removeLayer(getStrBool(keypts));
+ for (std::size_t i = 0; i < keypoints->size(); ++i) {
+ int u = int(keypoints->points[i].label % cloud->width);
+ int v = cloud->height - int(keypoints->points[i].label / cloud->width);
+ image_viewer_.markPoint(u,
+ v,
+ visualization::red_color,
+ visualization::blue_color,
+ 10,
+ getStrBool(!keypts));
}
+ keypts = !keypts;
+
+ visualization::PointCloudColorHandlerCustom<KeyPointT> blue(
+ keypoints, 0, 0, 255);
+ if (!cloud_viewer_.updatePointCloud(keypoints, blue, "keypoints"))
+ cloud_viewer_.addPointCloud(keypoints, blue, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties(
+ visualization::PCL_VISUALIZER_POINT_SIZE, 20, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties(
+ visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
+ }
+ }
- if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud"))
- {
- cloud_viewer_.addPointCloud (cloud, "OpenNICloud");
- cloud_viewer_.resetCameraViewpoint ("OpenNICloud");
- }
+ cloud_viewer_.spinOnce();
+ image_viewer_.spinOnce();
+ std::this_thread::sleep_for(100us);
+ }
- if (!image_init)
- {
- image_viewer_.setPosition (cloud->width, 0);
- image_viewer_.setSize (cloud->width, cloud->height);
- image_init = true;
- }
+ grabber_.stop();
+ cloud_connection.disconnect();
+ }
- image_viewer_.addRGBImage<PointT> (cloud);
-
- if (keypoints && !keypoints->empty ())
- {
- image_viewer_.removeLayer (getStrBool (keypts));
- for (std::size_t i = 0; i < keypoints->size (); ++i)
- {
- int u = int (keypoints->points[i].label % cloud->width);
- int v = cloud->height - int (keypoints->points[i].label / cloud->width);
- image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 10, getStrBool (!keypts));
- }
- keypts = !keypts;
-
- visualization::PointCloudColorHandlerCustom<KeyPointT> blue (keypoints, 0, 0, 255);
- if (!cloud_viewer_.updatePointCloud (keypoints, blue, "keypoints"))
- cloud_viewer_.addPointCloud (keypoints, blue, "keypoints");
- cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 20, "keypoints");
- cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
- }
- }
+ visualization::PCLVisualizer cloud_viewer_;
+ Grabber& grabber_;
+ std::mutex cloud_mutex_;
+ CloudConstPtr cloud_;
- cloud_viewer_.spinOnce ();
- image_viewer_.spinOnce ();
- std::this_thread::sleep_for(100us);
- }
+ visualization::ImageViewer image_viewer_;
- grabber_.stop ();
- cloud_connection.disconnect ();
- }
-
- visualization::PCLVisualizer cloud_viewer_;
- Grabber& grabber_;
- std::mutex cloud_mutex_;
- CloudConstPtr cloud_;
-
- visualization::ImageViewer image_viewer_;
-
- PointCloud<KeyPointT>::Ptr keypoints_;
-
- private:
- boost::signals2::connection cloud_connection;
+ PointCloud<KeyPointT>::Ptr keypoints_;
+
+private:
+ boost::signals2::connection cloud_connection;
};
/* ---[ */
int
-main (int, char**)
+main(int, char**)
{
- string device_id ("#1");
- OpenNIGrabber grabber (device_id);
- SUSANDemo openni_viewer (grabber);
-
- openni_viewer.init ();
- openni_viewer.run ();
-
- return (0);
+ string device_id("#1");
+ OpenNIGrabber grabber(device_id);
+ SUSANDemo openni_viewer(grabber);
+
+ openni_viewer.init();
+ openni_viewer.run();
+
+ return 0;
}
/* ]--- */
#include <pcl/apps/timer.h>
#include <pcl/common/common.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/keypoints/trajkovic_2d.h>
#include <pcl/keypoints/trajkovic_3d.h>
-#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
+#include <pcl/visualization/pcl_visualizer.h>
#include <mutex>
#include <thread>
using KeyPointT = PointXYZI;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-class TrajkovicDemo
-{
- public:
- using Cloud = PointCloud<PointT>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
-
- TrajkovicDemo (Grabber& grabber, bool enable_3d)
- : cloud_viewer_ ("TRAJKOVIC 3D Keypoints -- PointCloud")
- , grabber_ (grabber)
- , image_viewer_ ("TRAJKOVIC 3D Keypoints -- Image")
- , enable_3d_ (enable_3d)
- {
- }
+class TrajkovicDemo {
+public:
+ using Cloud = PointCloud<PointT>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ TrajkovicDemo(Grabber& grabber, bool enable_3d)
+ : cloud_viewer_("TRAJKOVIC 3D Keypoints -- PointCloud")
+ , grabber_(grabber)
+ , image_viewer_("TRAJKOVIC 3D Keypoints -- Image")
+ , enable_3d_(enable_3d)
+ {}
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ cloud_callback_3d(const CloudConstPtr& cloud)
+ {
+ FPS_CALC("cloud callback");
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ cloud_ = cloud;
+
+ // Compute TRAJKOVIC keypoints 3D
+ TrajkovicKeypoint3D<PointT, KeyPointT> trajkovic;
+ trajkovic.setInputCloud(cloud);
+ trajkovic.setNumberOfThreads(6);
+ keypoints_.reset(new PointCloud<KeyPointT>);
+ trajkovic.compute(*keypoints_);
+ keypoints_indices_ = trajkovic.getKeypointsIndices();
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- cloud_callback_3d (const CloudConstPtr& cloud)
- {
- FPS_CALC ("cloud callback");
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- cloud_ = cloud;
-
- // Compute TRAJKOVIC keypoints 3D
-
TrajkovicKeypoint3D<PointT, KeyPointT> trajkovic;
- trajkovic.setInputCloud (cloud);
- trajkovic.setNumberOfThreads (6);
- keypoints_.reset (new PointCloud<KeyPointT>);
- trajkovic.compute (*keypoints_);
- keypoints_indices_ = trajkovic.getKeypointsIndices ();
- }
+ /////////////////////////////////////////////////////////////////////////
+ void
+ cloud_callback_2d(const CloudConstPtr& cloud)
+ {
+ FPS_CALC("cloud callback");
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ cloud_ = cloud;
+
+ // Compute TRAJKOVIC keypoints 2D
+
TrajkovicKeypoint2D<PointT, KeyPointT> trajkovic;
+ trajkovic.setInputCloud(cloud);
+ trajkovic.setNumberOfThreads(6);
+ keypoints_.reset(new PointCloud<KeyPointT>);
+ trajkovic.compute(*keypoints_);
+ keypoints_indices_ = trajkovic.getKeypointsIndices();
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- cloud_callback_2d (const CloudConstPtr& cloud)
- {
- FPS_CALC ("cloud callback");
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- cloud_ = cloud;
-
- // Compute TRAJKOVIC keypoints 2D
- TrajkovicKeypoint2D<PointT, KeyPointT> trajkovic;
- trajkovic.setInputCloud (cloud);
- trajkovic.setNumberOfThreads (6);
- keypoints_.reset (new PointCloud<KeyPointT>);
- trajkovic.compute (*keypoints_);
- keypoints_indices_ = trajkovic.getKeypointsIndices ();
- }
+ /////////////////////////////////////////////////////////////////////////
+ void
+ init()
+ {
+ std::function<void(const CloudConstPtr&)> cloud_cb;
+ if (enable_3d_)
+ cloud_cb = [this](const CloudConstPtr& cloud) { cloud_callback_3d(cloud); };
+ else
+ cloud_cb = [this](const CloudConstPtr& cloud) { cloud_callback_2d(cloud); };
- /////////////////////////////////////////////////////////////////////////
- void
- init ()
- {
- std::function<void (const CloudConstPtr&) > cloud_cb;
- if (enable_3d_)
- cloud_cb = [this] (const CloudConstPtr& cloud) { cloud_callback_3d (cloud); };
- else
- cloud_cb = [this] (const CloudConstPtr& cloud) { cloud_callback_2d (cloud); };
-
- cloud_connection = grabber_.registerCallback (cloud_cb);
- }
+ cloud_connection = grabber_.registerCallback(cloud_cb);
+ }
- /////////////////////////////////////////////////////////////////////////
- std::string
- getStrBool (bool state)
- {
- std::ostringstream ss;
- ss << state;
- return (ss.str ());
- }
+ /////////////////////////////////////////////////////////////////////////
+ std::string
+ getStrBool(bool state)
+ {
+ std::ostringstream ss;
+ ss << state;
+ return ss.str();
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- run ()
- {
- grabber_.start ();
-
- bool image_init = false, cloud_init = false;
- bool keypts = true;
-
- while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ())
- {
- PointCloud<KeyPointT>::Ptr keypoints;
- CloudConstPtr cloud;
- if (cloud_mutex_.try_lock ())
- {
- cloud_.swap (cloud);
- keypoints_.swap (keypoints);
-
- cloud_mutex_.unlock ();
- }
+ /////////////////////////////////////////////////////////////////////////
+ void
+ run()
+ {
+ grabber_.start();
- if (cloud)
- {
- if (!cloud_init)
- {
- cloud_viewer_.setPosition (0, 0);
- cloud_viewer_.setSize (cloud->width, cloud->height);
- cloud_init = true;
- }
+ bool image_init = false, cloud_init = false;
+ bool keypts = true;
- if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud"))
- {
- cloud_viewer_.addPointCloud (cloud, "OpenNICloud");
- cloud_viewer_.resetCameraViewpoint ("OpenNICloud");
- }
+ while (!cloud_viewer_.wasStopped() && !image_viewer_.wasStopped()) {
+ PointCloud<KeyPointT>::Ptr keypoints;
+ CloudConstPtr cloud;
+ if (cloud_mutex_.try_lock()) {
+ cloud_.swap(cloud);
+ keypoints_.swap(keypoints);
- if (!image_init)
- {
- image_viewer_.setPosition (cloud->width, 0);
- image_viewer_.setSize (cloud->width, cloud->height);
- image_init = true;
- }
+ cloud_mutex_.unlock();
+ }
- image_viewer_.addRGBImage<PointT> (cloud);
-
- if (keypoints && !keypoints->empty ())
- {
- image_viewer_.removeLayer (getStrBool (keypts));
- std::vector<int> uv;
- uv.reserve (keypoints_indices_->indices.size () * 2);
- for (const int &index : keypoints_indices_->indices)
- {
- int u (index % cloud->width);
- int v (index / cloud->width);
- image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 5, getStrBool (!keypts), 0.5);
- }
- keypts = !keypts;
-
- visualization::PointCloudColorHandlerCustom<KeyPointT> blue (keypoints, 0, 0, 255);
- if (!cloud_viewer_.updatePointCloud (keypoints, blue, "keypoints"))
- cloud_viewer_.addPointCloud (keypoints, blue, "keypoints");
- cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 10, "keypoints");
- cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
- }
+ if (cloud) {
+ if (!cloud_init) {
+ cloud_viewer_.setPosition(0, 0);
+ cloud_viewer_.setSize(cloud->width, cloud->height);
+ cloud_init = true;
+ }
+
+ if (!cloud_viewer_.updatePointCloud(cloud, "OpenNICloud")) {
+ cloud_viewer_.addPointCloud(cloud, "OpenNICloud");
+ cloud_viewer_.resetCameraViewpoint("OpenNICloud");
+ }
+
+ if (!image_init) {
+ image_viewer_.setPosition(cloud->width, 0);
+ image_viewer_.setSize(cloud->width, cloud->height);
+ image_init = true;
}
- cloud_viewer_.spinOnce ();
- image_viewer_.spinOnce ();
- std::this_thread::sleep_for(100us);
+ image_viewer_.addRGBImage<PointT>(cloud);
+
+ if (keypoints && !keypoints->empty()) {
+ image_viewer_.removeLayer(getStrBool(keypts));
+ std::vector<int> uv;
+ uv.reserve(keypoints_indices_->indices.size() * 2);
+ for (const int& index : keypoints_indices_->indices) {
+ int u(index % cloud->width);
+ int v(index / cloud->width);
+ image_viewer_.markPoint(u,
+ v,
+ visualization::red_color,
+ visualization::blue_color,
+ 5,
+ getStrBool(!keypts),
+ 0.5);
+ }
+ keypts = !keypts;
+
+ visualization::PointCloudColorHandlerCustom<KeyPointT> blue(
+ keypoints, 0, 0, 255);
+ if (!cloud_viewer_.updatePointCloud(keypoints, blue, "keypoints"))
+ cloud_viewer_.addPointCloud(keypoints, blue, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties(
+ visualization::PCL_VISUALIZER_POINT_SIZE, 10, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties(
+ visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
+ }
}
- grabber_.stop ();
- cloud_connection.disconnect ();
+ cloud_viewer_.spinOnce();
+ image_viewer_.spinOnce();
+ std::this_thread::sleep_for(100us);
}
- visualization::PCLVisualizer cloud_viewer_;
- Grabber& grabber_;
- std::mutex cloud_mutex_;
- CloudConstPtr cloud_;
+ grabber_.stop();
+ cloud_connection.disconnect();
+ }
+
+ visualization::PCLVisualizer cloud_viewer_;
+ Grabber& grabber_;
+ std::mutex cloud_mutex_;
+ CloudConstPtr cloud_;
- visualization::ImageViewer image_viewer_;
+ visualization::ImageViewer image_viewer_;
- PointCloud<KeyPointT>::Ptr keypoints_;
- pcl::PointIndicesConstPtr keypoints_indices_;
- bool enable_3d_;
- private:
- boost::signals2::connection cloud_connection;
+ PointCloud<KeyPointT>::Ptr keypoints_;
+ pcl::PointIndicesConstPtr keypoints_indices_;
+ bool enable_3d_;
+
+private:
+ boost::signals2::connection cloud_connection;
};
/* ---[ */
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
- if (pcl::console::find_switch (argc, argv, "-h"))
- {
- pcl::console::print_info ("Syntax is: %s [-device device_id_string] [-2d]\n", argv[0]);
- return (0);
+ if (pcl::console::find_switch(argc, argv, "-h")) {
+ pcl::console::print_info("Syntax is: %s [-device device_id_string] [-2d]\n",
+ argv[0]);
+ return 0;
}
- std::string device_id ("#1");
+ std::string device_id("#1");
bool enable_3d = true;
- if (pcl::console::find_switch (argc, argv, "-2d"))
+ if (pcl::console::find_switch(argc, argv, "-2d"))
enable_3d = false;
- if (pcl::console::find_argument (argc, argv, "-device"))
- pcl::console::parse<std::string> (argc, argv, "-device", device_id);
+ if (pcl::console::find_argument(argc, argv, "-device"))
+ pcl::console::parse<std::string>(argc, argv, "-device", device_id);
- pcl::console::print_info ("Extracting Trajkovic %s keypoints from device %s.\n",
- enable_3d ? "3D" : "2D", device_id.c_str ());
+ pcl::console::print_info("Extracting Trajkovic %s keypoints from device %s.\n",
+ enable_3d ? "3D" : "2D",
+ device_id.c_str());
- OpenNIGrabber grabber (device_id);
+ OpenNIGrabber grabber(device_id);
- TrajkovicDemo openni_viewer (grabber, enable_3d);
+ TrajkovicDemo openni_viewer(grabber, enable_3d);
- openni_viewer.init ();
- openni_viewer.run ();
+ openni_viewer.init();
+ openni_viewer.run();
- return (0);
+ return 0;
}
/* ]--- */
*
*/
-#include <pcl/point_types.h>
-#include <pcl/io/pcd_io.h>
#include <pcl/apps/vfh_nn_classifier.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
-int
-main (int, char* argv[])
+int
+main(int, char* argv[])
{
// Load input file
char* file_name = argv[1];
pcl::PCLPointCloud2 cloud_blob;
- pcl::io::loadPCDFile (file_name, cloud_blob);
-
- // Declare variable to hold result
- pcl::NNClassification<pcl::VFHSignature308>::ResultPtr result;
- // same as: pcl::VFHClassifierNN::ResultPtr result;
-
- // Do general classification using NNClassification or use the VHClassiierNN helper class
- if (false)
- {
- // Estimate your favorite feature
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
- pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
- /// NOTE: make sure to use same radius as for training data
- pcl::PointCloud<pcl::VFHSignature308>::Ptr feature = pcl::computeVFH<pcl::PointXYZ> (cloud, 0.03);
+ pcl::io::loadPCDFile(file_name, cloud_blob);
- // Nearest neighbors classification
- pcl::NNClassification<pcl::VFHSignature308> nn;
- //nn.setTrainingFeatures(cloud);
- //nn.setTrainingLabels(std::vector<std::string>(cloud->points.size(), "bla"));
- nn.loadTrainingFeatures (argv[2], argv[3]);
- result = nn.classify(feature->points[0], 300, 50);
- }
- else
- {
- pcl::VFHClassifierNN vfh_classifier;
- //vfh_classifier.loadTrainingData ("/home/marton/ros/pcl/trunk/apps/data/can.pcd", "can");
- //vfh_classifier.loadTrainingData ("/home/marton/ros/pcl/trunk/apps/data/salt.pcd", "salt");
- //vfh_classifier.loadTrainingData ("/home/marton/ros/pcl/trunk/apps/data/sugar.pcd", "sugar");
- //vfh_classifier.saveTrainingFeatures ("/tmp/vfhs.pcd", "/tmp/vfhs.labels");
- vfh_classifier.loadTrainingFeatures (argv[2], argv[3]);
- vfh_classifier.finalizeTraining ();
- result = vfh_classifier.classify(cloud_blob);
- }
+ // Do general classification using VFHClassifierNN
+ pcl::VFHClassifierNN vfh_classifier;
+ vfh_classifier.loadTrainingFeatures(argv[2], argv[3]);
+ vfh_classifier.finalizeTraining();
+ const auto result = vfh_classifier.classify(cloud_blob);
// Print results
for (std::size_t i = 0; i < result->first.size(); ++i)
- std::cerr << result->first.at (i) << ": " << result->second.at (i) << std::endl;
+ std::cerr << result->first.at(i) << ": " << result->second.at(i) << std::endl;
return 0;
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/concave_hull.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
#include <thread>
using namespace std;
using namespace std::chrono_literals;
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- if (++count == 100) \
- { \
- double now = pcl::getTime (); \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ if (++count == 100) { \
+ double now = pcl::getTime(); \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
template <typename PointType>
-class OpenNI3DConcaveHull
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNI3DConcaveHull (const std::string& device_id = "")
- : viewer ("PCL OpenNI 3D Concave Hull Viewer")
- , device_id_(device_id)
- {
- grid.setFilterFieldName ("z");
- grid.setFilterLimits (0.0, 1.0);
- grid.setLeafSize (0.01f, 0.01f, 0.01f);
- }
-
- void
- cloud_cb (const CloudConstPtr& cloud)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- FPS_CALC ("computation");
-
- cloud_pass_.reset (new Cloud);
- // Computation goes here
- grid.setInputCloud (cloud);
- grid.filter (*cloud_pass_);
-
- // Estimate 3D convex hull
- pcl::ConcaveHull<PointType> hr;
- hr.setAlpha (0.1);
- hr.setInputCloud (cloud_pass_);
- cloud_hull_.reset (new Cloud);
- hr.reconstruct (*cloud_hull_, vertices_);
-
- cloud_ = cloud;
- new_cloud_ = true;
+class OpenNI3DConcaveHull {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNI3DConcaveHull(const std::string& device_id = "")
+ : viewer("PCL OpenNI 3D Concave Hull Viewer"), device_id_(device_id)
+ {
+ grid.setFilterFieldName("z");
+ grid.setFilterLimits(0.0, 1.0);
+ grid.setLeafSize(0.01f, 0.01f, 0.01f);
+ }
+
+ void
+ cloud_cb(const CloudConstPtr& cloud)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ FPS_CALC("computation");
+
+ cloud_pass_.reset(new Cloud);
+ // Computation goes here
+ grid.setInputCloud(cloud);
+ grid.filter(*cloud_pass_);
+
+ // Estimate 3D convex hull
+ pcl::ConcaveHull<PointType> hr;
+ hr.setAlpha(0.1);
+ hr.setInputCloud(cloud_pass_);
+ cloud_hull_.reset(new Cloud);
+ hr.reconstruct(*cloud_hull_, vertices_);
+
+ cloud_ = cloud;
+ new_cloud_ = true;
+ }
+
+ void
+ viz_cb(pcl::visualization::PCLVisualizer& viz)
+ {
+ if (!cloud_ || !new_cloud_) {
+ std::this_thread::sleep_for(1ms);
+ return;
}
- void
- viz_cb (pcl::visualization::PCLVisualizer& viz)
{
- if (!cloud_ || !new_cloud_)
- {
- std::this_thread::sleep_for(1ms);
- return;
+ std::lock_guard<std::mutex> lock(mtx_);
+ FPS_CALC("visualization");
+ CloudPtr temp_cloud;
+ temp_cloud.swap(cloud_pass_);
+
+ if (!viz.updatePointCloud(temp_cloud, "OpenNICloud")) {
+ viz.addPointCloud(temp_cloud, "OpenNICloud");
+ viz.resetCameraViewpoint("OpenNICloud");
}
-
- {
- std::lock_guard<std::mutex> lock (mtx_);
- FPS_CALC ("visualization");
- CloudPtr temp_cloud;
- temp_cloud.swap (cloud_pass_);
-
- if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
- {
- viz.addPointCloud (temp_cloud, "OpenNICloud");
- viz.resetCameraViewpoint ("OpenNICloud");
- }
- // Render the data
- if (new_cloud_ && cloud_hull_)
- {
- viz.removePointCloud ("hull");
- viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
- }
- new_cloud_ = false;
+ // Render the data
+ if (new_cloud_ && cloud_hull_) {
+ viz.removePointCloud("hull");
+ viz.addPolygonMesh<PointType>(cloud_hull_, vertices_, "hull");
}
+ new_cloud_ = false;
}
+ }
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- viewer.runOnVisualizationThread ([this] (pcl::visualization::PCLVisualizer& viz) { viz_cb (viz); }, "viz_cb");
+ viewer.runOnVisualizationThread(
+ [this](pcl::visualization::PCLVisualizer& viz) { viz_cb(viz); }, "viz_cb");
- interface.start ();
-
- while (!viewer.wasStopped ())
- {
- std::this_thread::sleep_for(1ms);
- }
+ interface.start();
- interface.stop ();
+ while (!viewer.wasStopped()) {
+ std::this_thread::sleep_for(1ms);
}
- pcl::VoxelGrid<PointType> grid;
- pcl::visualization::CloudViewer viewer;
+ interface.stop();
+ }
+
+ pcl::VoxelGrid<PointType> grid;
+ pcl::visualization::CloudViewer viewer;
- std::string device_id_;
- std::mutex mtx_;
- // Data
- CloudConstPtr cloud_;
- CloudPtr cloud_pass_, cloud_hull_;
-
std::vector<pcl::Vertices> vertices_;
- bool new_cloud_;
+ std::string device_id_;
+ std::mutex mtx_;
+ // Data
+ CloudConstPtr cloud_;
+ CloudPtr cloud_pass_, cloud_hull_;
+ std::vector<pcl::Vertices> vertices_;
+ bool new_cloud_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
std::string arg;
if (argc > 1)
- arg = std::string (argv[1]);
-
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ arg = std::string(argv[1]);
+
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
- pcl::OpenNIGrabber grabber ("");
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- PCL_INFO ("PointXYZRGBA mode enabled.\n");
- OpenNI3DConcaveHull<pcl::PointXYZRGBA> v ("");
- v.run ();
+ pcl::OpenNIGrabber grabber("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ PCL_INFO("PointXYZRGBA mode enabled.\n");
+ OpenNI3DConcaveHull<pcl::PointXYZRGBA> v("");
+ v.run();
}
- else
- {
- PCL_INFO ("PointXYZ mode enabled.\n");
- OpenNI3DConcaveHull<pcl::PointXYZ> v ("");
- v.run ();
+ else {
+ PCL_INFO("PointXYZ mode enabled.\n");
+ OpenNI3DConcaveHull<pcl::PointXYZ> v("");
+ v.run();
}
- return (0);
+ return 0;
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/filters/passthrough.h>
#include <pcl/surface/convex_hull.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
#include <thread>
using namespace pcl;
using namespace pcl::visualization;
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- if (++count == 100) \
- { \
- double now = pcl::getTime (); \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ if (++count == 100) { \
+ double now = pcl::getTime(); \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
template <typename PointType>
-class OpenNI3DConvexHull
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNI3DConvexHull (const std::string& device_id = "")
- : viewer ("PCL OpenNI 3D Convex Hull Viewer")
- , device_id_(device_id)
- {
- pass.setFilterFieldName ("z");
- pass.setFilterLimits (0.0, 1.0);
- }
-
- void
- cloud_cb (const CloudConstPtr& cloud)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- FPS_CALC ("computation");
-
- cloud_pass_.reset (new Cloud);
- // Computation goes here
- pass.setInputCloud (cloud);
- pass.filter (*cloud_pass_);
-
- // Estimate 3D convex hull
- pcl::ConvexHull<PointType> hr;
- hr.setInputCloud (cloud_pass_);
- cloud_hull_.reset (new Cloud);
- hr.reconstruct (*cloud_hull_, vertices_);
-
- cloud_ = cloud;
- new_cloud_ = true;
+class OpenNI3DConvexHull {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNI3DConvexHull(const std::string& device_id = "")
+ : viewer("PCL OpenNI 3D Convex Hull Viewer"), device_id_(device_id)
+ {
+ pass.setFilterFieldName("z");
+ pass.setFilterLimits(0.0, 1.0);
+ }
+
+ void
+ cloud_cb(const CloudConstPtr& cloud)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ FPS_CALC("computation");
+
+ cloud_pass_.reset(new Cloud);
+ // Computation goes here
+ pass.setInputCloud(cloud);
+ pass.filter(*cloud_pass_);
+
+ // Estimate 3D convex hull
+ pcl::ConvexHull<PointType> hr;
+ hr.setInputCloud(cloud_pass_);
+ cloud_hull_.reset(new Cloud);
+ hr.reconstruct(*cloud_hull_, vertices_);
+
+ cloud_ = cloud;
+ new_cloud_ = true;
+ }
+
+ void
+ viz_cb(pcl::visualization::PCLVisualizer& viz)
+ {
+ if (!cloud_ || !new_cloud_) {
+ std::this_thread::sleep_for(1ms);
+ return;
}
- void
- viz_cb (pcl::visualization::PCLVisualizer& viz)
{
- if (!cloud_ || !new_cloud_)
- {
- std::this_thread::sleep_for(1ms);
- return;
+ std::lock_guard<std::mutex> lock(mtx_);
+ FPS_CALC("visualization");
+ CloudPtr temp_cloud;
+ temp_cloud.swap(cloud_pass_);
+
+ if (!viz.updatePointCloud(temp_cloud, "OpenNICloud")) {
+ viz.addPointCloud(temp_cloud, "OpenNICloud");
+ viz.resetCameraViewpoint("OpenNICloud");
}
-
- {
- std::lock_guard<std::mutex> lock (mtx_);
- FPS_CALC ("visualization");
- CloudPtr temp_cloud;
- temp_cloud.swap (cloud_pass_);
-
- if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
- {
- viz.addPointCloud (temp_cloud, "OpenNICloud");
- viz.resetCameraViewpoint ("OpenNICloud");
- }
- // Render the data
- if (new_cloud_ && cloud_hull_)
- {
- viz.removePointCloud ("hull");
- viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
- }
- new_cloud_ = false;
+ // Render the data
+ if (new_cloud_ && cloud_hull_) {
+ viz.removePointCloud("hull");
+ viz.addPolygonMesh<PointType>(cloud_hull_, vertices_, "hull");
}
+ new_cloud_ = false;
}
+ }
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- viewer.runOnVisualizationThread ([this] (pcl::visualization::PCLVisualizer& viz) { viz_cb (viz); }, "viz_cb");
+ viewer.runOnVisualizationThread(
+ [this](pcl::visualization::PCLVisualizer& viz) { viz_cb(viz); }, "viz_cb");
- interface.start ();
-
- while (!viewer.wasStopped ())
- {
- std::this_thread::sleep_for(1ms);
- }
+ interface.start();
- interface.stop ();
+ while (!viewer.wasStopped()) {
+ std::this_thread::sleep_for(1ms);
}
- pcl::PassThrough<PointType> pass;
- pcl::visualization::CloudViewer viewer;
+ interface.stop();
+ }
+
+ pcl::PassThrough<PointType> pass;
+ pcl::visualization::CloudViewer viewer;
- std::string device_id_;
- std::mutex mtx_;
- // Data
- CloudConstPtr cloud_;
- CloudPtr cloud_pass_, cloud_hull_;
-
std::vector<pcl::Vertices> vertices_;
- bool new_cloud_;
+ std::string device_id_;
+ std::mutex mtx_;
+ // Data
+ CloudConstPtr cloud_;
+ CloudPtr cloud_pass_, cloud_hull_;
+ std::vector<pcl::Vertices> vertices_;
+ bool new_cloud_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
std::string arg;
if (argc > 1)
- arg = std::string (argv[1]);
-
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ arg = std::string(argv[1]);
+
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
- pcl::OpenNIGrabber grabber ("");
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- PCL_INFO ("PointXYZRGBA mode enabled.\n");
- OpenNI3DConvexHull<pcl::PointXYZRGBA> v ("");
- v.run ();
+ pcl::OpenNIGrabber grabber("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ PCL_INFO("PointXYZRGBA mode enabled.\n");
+ OpenNI3DConvexHull<pcl::PointXYZRGBA> v("");
+ v.run();
}
- else
- {
- PCL_INFO ("PointXYZ mode enabled.\n");
- OpenNI3DConvexHull<pcl::PointXYZ> v ("");
- v.run ();
+ else {
+ PCL_INFO("PointXYZ mode enabled.\n");
+ OpenNI3DConvexHull<pcl::PointXYZ> v("");
+ v.run();
}
- return (0);
+ return 0;
}
*
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/filters/approximate_voxel_grid.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
#include <pcl/features/boundary.h>
#include <pcl/features/integral_image_normal.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <pcl/io/openni_camera/openni_driver.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
#include <thread>
using ColorHandlerPtr = ColorHandler::Ptr;
using ColorHandlerConstPtr = ColorHandler::ConstPtr;
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
-
-class OpenNIIntegralImageNormalEstimation
-{
- public:
- using Cloud = pcl::PointCloud<pcl::PointXYZRGBNormal>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
-
- OpenNIIntegralImageNormalEstimation (const std::string& device_id = "")
- : viewer ("PCL OpenNI NormalEstimation Viewer")
- , device_id_(device_id)
- {
- ne_.setNormalEstimationMethod (ne_.AVERAGE_3D_GRADIENT);
- //ne_.setNormalEstimationMethod (ne_.COVARIANCE_MATRIX);
- ne_.setRectSize (10, 10);
- new_cloud_ = false;
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
+
+class OpenNIIntegralImageNormalEstimation {
+public:
+ using Cloud = pcl::PointCloud<pcl::PointXYZRGBNormal>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ OpenNIIntegralImageNormalEstimation(const std::string& device_id = "")
+ : viewer("PCL OpenNI NormalEstimation Viewer"), device_id_(device_id)
+ {
+ ne_.setNormalEstimationMethod(ne_.AVERAGE_3D_GRADIENT);
+ ne_.setRectSize(10, 10);
+ new_cloud_ = false;
- pass_.setDownsampleAllData (true);
- pass_.setLeafSize (0.005f, 0.005f, 0.005f);
+ pass_.setDownsampleAllData(true);
+ pass_.setLeafSize(0.005f, 0.005f, 0.005f);
- pcl::search::OrganizedNeighbor<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBNormal>);
- be_.setRadiusSearch (0.02);
- be_.setSearchMethod (tree);
- }
-
- void
- cloud_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- //lock while we set our cloud;
- FPS_CALC ("computation");
-
- cloud_.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-
- // Estimate surface normals
- ne_.setInputCloud (cloud);
- ne_.compute (*cloud_);
- copyPointCloud (*cloud, *cloud_);
-
-// cloud_pass_.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
- // Passthrough
-// pass_.setInputCloud (cloud_);
-// pass_.filter (*cloud_pass_);
-
- be_.setInputCloud (cloud_);
- be_.setInputNormals (cloud_);
- boundaries_.reset (new pcl::PointCloud<pcl::Boundary>);
- be_.compute (*boundaries_);
-
- new_cloud_ = true;
+ pcl::search::OrganizedNeighbor<pcl::PointXYZRGBNormal>::Ptr tree(
+ new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBNormal>);
+ be_.setRadiusSearch(0.02);
+ be_.setSearchMethod(tree);
+ }
+
+ void
+ cloud_cb(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ // lock while we set our cloud;
+ FPS_CALC("computation");
+
+ cloud_.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+ // Estimate surface normals
+ ne_.setInputCloud(cloud);
+ ne_.compute(*cloud_);
+ copyPointCloud(*cloud, *cloud_);
+
+ be_.setInputCloud(cloud_);
+ be_.setInputNormals(cloud_);
+ boundaries_.reset(new pcl::PointCloud<pcl::Boundary>);
+ be_.compute(*boundaries_);
+
+ new_cloud_ = true;
+ }
+
+ void
+ viz_cb(pcl::visualization::PCLVisualizer& viz)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ if (!cloud_) {
+ std::this_thread::sleep_for(1s);
+ return;
}
- void
- viz_cb (pcl::visualization::PCLVisualizer& viz)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- if (!cloud_)
- {
- std::this_thread::sleep_for(1s);
- return;
- }
-
- // Render the data
- if (new_cloud_ && cloud_ && boundaries_)
- {
- CloudPtr temp_cloud;
- temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
-
-// if (!viz.updatePointCloud<pcl::PointXYZRGBNormal> (temp_cloud, "OpenNICloud"))
-// {
-// viz.addPointCloud<pcl::PointXYZRGBNormal> (temp_cloud, "OpenNICloud");
-// viz.resetCameraViewpoint ("OpenNICloud");
-// }
-
- viz.removePointCloud ("normalcloud");
- pcl::PCLPointCloud2::Ptr cloud2 (new pcl::PCLPointCloud2);
- pcl::toPCLPointCloud2 (*boundaries_, *cloud2);
- ColorHandlerConstPtr color_handler (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (cloud2, "boundary_point"));
- viz.addPointCloud<pcl::PointXYZRGBNormal> (temp_cloud, color_handler, "normalcloud");
- viz.resetCameraViewpoint ("normalcloud");
- new_cloud_ = false;
- }
+ // Render the data
+ if (new_cloud_ && cloud_ && boundaries_) {
+ CloudPtr temp_cloud;
+ temp_cloud.swap(cloud_); // here we set cloud_ to null, so that
+
+ viz.removePointCloud("normalcloud");
+ pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
+ pcl::toPCLPointCloud2(*boundaries_, *cloud2);
+ ColorHandlerConstPtr color_handler(
+ new pcl::visualization::PointCloudColorHandlerGenericField<
+ pcl::PCLPointCloud2>(cloud2, "boundary_point"));
+ viz.addPointCloud<pcl::PointXYZRGBNormal>(
+ temp_cloud, color_handler, "normalcloud");
+ viz.resetCameraViewpoint("normalcloud");
+ new_cloud_ = false;
}
+ }
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
- std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &)> f = [this] (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud) { cloud_cb (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
+ std::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =
+ [this](const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- viewer.runOnVisualizationThread ([this] (pcl::visualization::PCLVisualizer& viz) { viz_cb (viz); }, "viz_cb");
+ viewer.runOnVisualizationThread(
+ [this](pcl::visualization::PCLVisualizer& viz) { viz_cb(viz); }, "viz_cb");
- interface.start ();
-
- while (!viewer.wasStopped ())
- {
- std::this_thread::sleep_for(1s);
- }
+ interface.start();
- interface.stop ();
+ while (!viewer.wasStopped()) {
+ std::this_thread::sleep_for(1s);
}
- pcl::ApproximateVoxelGrid<pcl::PointXYZRGBNormal> pass_;
- pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne_;
- pcl::BoundaryEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::Boundary> be_;
- pcl::visualization::CloudViewer viewer;
- std::string device_id_;
- std::mutex mtx_;
- // Data
- pcl::PointCloud<pcl::Boundary>::Ptr boundaries_;
- CloudPtr cloud_, cloud_pass_;
- bool new_cloud_;
+ interface.stop();
+ }
+
+ pcl::ApproximateVoxelGrid<pcl::PointXYZRGBNormal> pass_;
+ pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne_;
+ pcl::BoundaryEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::Boundary>
+ be_;
+ pcl::visualization::CloudViewer viewer;
+ std::string device_id_;
+ std::mutex mtx_;
+ // Data
+ pcl::PointCloud<pcl::Boundary>::Ptr boundaries_;
+ CloudPtr cloud_, cloud_pass_;
+ bool new_cloud_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
std::cout << "No devices connected." << std::endl;
}
-int
-main (int argc, char ** argv)
+int
+main(int argc, char** argv)
{
std::string arg;
if (argc > 1)
- arg = std::string (argv[1]);
-
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ arg = std::string(argv[1]);
+
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
- pcl::OpenNIGrabber grabber ("");
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb> ())
- {
- OpenNIIntegralImageNormalEstimation v ("");
- v.run ();
+ pcl::OpenNIGrabber grabber("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb>()) {
+ OpenNIIntegralImageNormalEstimation v("");
+ v.run();
}
else
- PCL_ERROR ("The input device does not provide a PointXYZRGB mode.\n");
+ PCL_ERROR("The input device does not provide a PointXYZRGB mode.\n");
- return (0);
+ return 0;
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
* Author: Nico Blodow (blodow@cs.tum.edu), Julius Kammerl (julius@kammerl.de)
*/
-#include <thread>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
+#include <pcl/console/parse.h>
+#include <pcl/filters/extract_indices.h>
#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/cloud_viewer.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
-#include <pcl/filters/extract_indices.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
-#include <pcl/console/parse.h>
+#include <thread>
using namespace std::chrono_literals;
-enum
-{
- REDDIFF_MODE,
- ONLYDIFF_MODE,
- MODE_COUNT
-};
+enum { REDDIFF_MODE, ONLYDIFF_MODE, MODE_COUNT };
+
+class OpenNIChangeViewer {
+public:
+ OpenNIChangeViewer(double resolution, int mode, int noise_filter)
+ : viewer("PCL OpenNI Viewer")
+ {
+ octree =
+ new pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZRGBA>(resolution);
+ mode_ = mode;
+ noise_filter_ = noise_filter;
+ }
-class OpenNIChangeViewer
-{
- public:
- OpenNIChangeViewer (double resolution, int mode, int noise_filter)
- : viewer ("PCL OpenNI Viewer")
- {
- octree = new pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZRGBA>(resolution);
- mode_ = mode;
- noise_filter_ = noise_filter;
- }
+ void
+ cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
+ {
+ std::cerr << cloud->points.size() << " -- ";
- void
- cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
- {
- std::cerr << cloud->points.size() << " -- ";
+ // assign point cloud to octree
+ octree->setInputCloud(cloud);
- // assign point cloud to octree
- octree->setInputCloud (cloud);
+ // add points from cloud to octree
+ octree->addPointsFromInputCloud();
- // add points from cloud to octree
- octree->addPointsFromInputCloud ();
+ std::cerr << octree->getLeafCount() << " -- ";
+ std::vector<int> newPointIdxVector;
- std::cerr << octree->getLeafCount() << " -- ";
- std::vector<int> newPointIdxVector;
+ // get a vector of new points, which did not exist in previous buffer
+ octree->getPointIndicesFromNewVoxels(newPointIdxVector, noise_filter_);
- // get a vector of new points, which did not exist in previous buffer
- octree->getPointIndicesFromNewVoxels (newPointIdxVector, noise_filter_);
+ std::cerr << newPointIdxVector.size() << std::endl;
- std::cerr << newPointIdxVector.size() << std::endl;
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr filtered_cloud;
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr filtered_cloud;
+ switch (mode_) {
+ case REDDIFF_MODE:
+ filtered_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>(*cloud));
+ filtered_cloud->points.reserve(newPointIdxVector.size());
- switch (mode_)
- {
- case REDDIFF_MODE:
- filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA> (*cloud));
- filtered_cloud->points.reserve(newPointIdxVector.size());
+ for (const int& idx : newPointIdxVector)
+ filtered_cloud->points[idx].rgba = 255 << 16;
- for (const int &idx : newPointIdxVector)
- filtered_cloud->points[idx].rgba = 255<<16;
+ if (!viewer.wasStopped())
+ viewer.showCloud(filtered_cloud);
- if (!viewer.wasStopped())
- viewer.showCloud (filtered_cloud);
+ break;
+ case ONLYDIFF_MODE:
+ filtered_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
- break;
- case ONLYDIFF_MODE:
- filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
+ filtered_cloud->points.reserve(newPointIdxVector.size());
- filtered_cloud->points.reserve(newPointIdxVector.size());
+ for (const int& idx : newPointIdxVector)
+ filtered_cloud->points.push_back(cloud->points[idx]);
- for (const int &idx : newPointIdxVector)
- filtered_cloud->points.push_back(cloud->points[idx]);
+ if (!viewer.wasStopped())
+ viewer.showCloud(filtered_cloud);
+ break;
+ }
+ // switch buffers - reset tree
+ octree->switchBuffers();
+ }
- if (!viewer.wasStopped())
- viewer.showCloud (filtered_cloud);
- break;
- }
-
- // switch buffers - reset tree
- octree->switchBuffers ();
- }
-
- void
- run ()
- {
- pcl::OpenNIGrabber interface {};
-
- std::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
- [this] (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud) { cloud_cb_ (cloud); };
-
- boost::signals2::connection c = interface.registerCallback (f);
-
- interface.start ();
-
- while (!viewer.wasStopped())
- {
- std::this_thread::sleep_for(1s);
- }
-
- interface.stop ();
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{};
+
+ std::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
+ [this](const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+
+ boost::signals2::connection c = interface.registerCallback(f);
+
+ interface.start();
+
+ while (!viewer.wasStopped()) {
+ std::this_thread::sleep_for(1s);
}
- pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZRGBA> *octree;
- pcl::visualization::CloudViewer viewer;
+ interface.stop();
+ }
+
+ pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZRGBA>* octree;
+ pcl::visualization::CloudViewer viewer;
- int mode_;
- int noise_filter_;
+ int mode_;
+ int noise_filter_;
};
-int
-main (int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
- std::cout << "Syntax is " << argv[0] << " [-r octree resolution] [-d] [-n noise_filter intensity] \n";
+ std::cout << "Syntax is " << argv[0]
+ << " [-r octree resolution] [-d] [-n noise_filter intensity] \n";
int mode = REDDIFF_MODE;
int noise_filter = 7;
double resolution = 0.01;
- pcl::console::parse_argument (argc, argv, "-r", resolution);
+ pcl::console::parse_argument(argc, argv, "-r", resolution);
- pcl::console::parse_argument (argc, argv, "-n", noise_filter);
+ pcl::console::parse_argument(argc, argv, "-n", noise_filter);
- if (pcl::console::find_argument (argc, argv, "-d")>0) {
+ if (pcl::console::find_argument(argc, argv, "-d") > 0) {
mode = ONLYDIFF_MODE;
}
-
- OpenNIChangeViewer v (resolution, mode, noise_filter);
- v.run ();
+ OpenNIChangeViewer v(resolution, mode, noise_filter);
+ v.run();
return 0;
}
* Author: Suat Gedikli (gedikli@willowgarage.com)
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/filters/color.h>
+#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/filters/color.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
-
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
template <typename PointType>
-class OpenNIPassthrough
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNIPassthrough (pcl::OpenNIGrabber& grabber, unsigned char red, unsigned char green, unsigned char blue, unsigned char radius)
- : viewer ("PCL OpenNI ColorFilter Viewer")
- , grabber_(grabber)
- {
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb_ (cloud); };
- boost::signals2::connection c = grabber_.registerCallback (f);
-
- std::vector<bool> lookup(1<<24, false);
- fillLookup (lookup, red, green, blue, radius);
- unsigned set = 0;
- for (unsigned i = 0; i < (1<<24); ++i)
- if (lookup[i])
+class OpenNIPassthrough {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNIPassthrough(pcl::OpenNIGrabber& grabber,
+ unsigned char red,
+ unsigned char green,
+ unsigned char blue,
+ unsigned char radius)
+ : viewer("PCL OpenNI ColorFilter Viewer"), grabber_(grabber)
+ {
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+ boost::signals2::connection c = grabber_.registerCallback(f);
+
+ std::vector<bool> lookup(1 << 24, false);
+ fillLookup(lookup, red, green, blue, radius);
+ unsigned set = 0;
+ for (unsigned i = 0; i < (1 << 24); ++i)
+ if (lookup[i])
++set;
-
- std::cout << "used colors: " << set << std::endl;
-
- color_filter_.setLookUpTable (lookup);
- }
- void fillLookup (std::vector<bool>& lookup, unsigned char red, unsigned char green, unsigned char blue, unsigned radius)
- {
- unsigned radius_sqr = radius * radius;
- pcl::RGB color;
- for (color.rgba = 0; color.rgba < (1<<24); ++color.rgba)
- {
- unsigned dist = (unsigned(color.r) - unsigned(red)) * (unsigned(color.r) - unsigned(red)) +
- (unsigned(color.g) - unsigned(green)) * (unsigned(color.g) - unsigned(green)) +
- (unsigned(color.b) - unsigned(blue)) * (unsigned(color.b) - unsigned(blue));
- if (dist < radius_sqr)
- lookup [color.rgba] = true;
- else
- lookup [color.rgba] = false;
- }
- }
+ std::cout << "used colors: " << set << std::endl;
+
+ color_filter_.setLookUpTable(lookup);
+ }
- void
- cloud_cb_ (const CloudConstPtr& cloud)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- FPS_CALC ("computation");
-
- cloud_color_.reset (new Cloud);
- // Computation goes here
- color_filter_.setInputCloud (cloud);
- color_filter_.filter (*cloud_color_);
- cloud_ = cloud;
+ void
+ fillLookup(std::vector<bool>& lookup,
+ unsigned char red,
+ unsigned char green,
+ unsigned char blue,
+ unsigned radius)
+ {
+ unsigned radius_sqr = radius * radius;
+ pcl::RGB color;
+ for (color.rgba = 0; color.rgba < (1 << 24); ++color.rgba) {
+ unsigned dist =
+ (unsigned(color.r) - unsigned(red)) * (unsigned(color.r) - unsigned(red)) +
+ (unsigned(color.g) - unsigned(green)) *
+ (unsigned(color.g) - unsigned(green)) +
+ (unsigned(color.b) - unsigned(blue)) * (unsigned(color.b) - unsigned(blue));
+ if (dist < radius_sqr)
+ lookup[color.rgba] = true;
+ else
+ lookup[color.rgba] = false;
}
+ }
+
+ void
+ cloud_cb_(const CloudConstPtr& cloud)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ FPS_CALC("computation");
+
+ cloud_color_.reset(new Cloud);
+ // Computation goes here
+ color_filter_.setInputCloud(cloud);
+ color_filter_.filter(*cloud_color_);
+ cloud_ = cloud;
+ }
- void
- run ()
- {
+ void
+ run()
+ {
- grabber_.start ();
+ grabber_.start();
- while (!viewer.wasStopped ())
- {
- if (cloud_color_)
- {
- std::lock_guard<std::mutex> lock (mtx_);
+ while (!viewer.wasStopped()) {
+ if (cloud_color_) {
+ std::lock_guard<std::mutex> lock(mtx_);
- FPS_CALC ("visualization");
- CloudPtr temp_cloud;
- temp_cloud.swap (cloud_color_); //here we set cloud_ to null, so that
- viewer.showCloud (temp_cloud);
- }
+ FPS_CALC("visualization");
+ CloudPtr temp_cloud;
+ temp_cloud.swap(cloud_color_); // here we set cloud_ to null, so that
+ viewer.showCloud(temp_cloud);
}
-
- grabber_.stop ();
}
- pcl::ColorFilter<PointType> color_filter_;
- pcl::visualization::CloudViewer viewer;
- pcl::OpenNIGrabber& grabber_;
- std::string device_id_;
- std::mutex mtx_;
- CloudConstPtr cloud_;
- CloudPtr cloud_color_;
+ grabber_.stop();
+ }
+
+ pcl::ColorFilter<PointType> color_filter_;
+ pcl::visualization::CloudViewer viewer;
+ pcl::OpenNIGrabber& grabber_;
+ std::string device_id_;
+ std::mutex mtx_;
+ CloudConstPtr cloud_;
+ CloudPtr cloud_color_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
- std::cout << "usage: " << argv[0] << " <device_id> [-rgb <red> <green> <blue> [-radius <radius>] ]\n\n" << std::endl;
-
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ std::cout << "usage: " << argv[0]
+ << " <device_id> [-rgb <red> <green> <blue> [-radius <radius>] ]\n\n"
+ << std::endl;
+
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << (int)driver.getBus (deviceIdx) << " @ " << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
- if (argc < 2)
- {
- usage (argv);
+ if (argc < 2) {
+ usage(argv);
return 1;
}
- std::string arg (argv[1]);
+ std::string arg(argv[1]);
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
int rr, gg, bb;
unsigned char radius = 442; // all colors!
- if (pcl::console::parse_3x_arguments (argc, argv, "-rgb", rr, gg, bb, true) != -1 )
- {
+ if (pcl::console::parse_3x_arguments(argc, argv, "-rgb", rr, gg, bb, true) != -1) {
std::cout << "-rgb present" << std::endl;
int rad;
- int idx = pcl::console::parse_argument (argc, argv, "-radius", rad);
- if (idx != -1)
- {
+ int idx = pcl::console::parse_argument(argc, argv, "-radius", rad);
+ if (idx != -1) {
if (rad > 0)
radius = rad;
}
if (rr >= 0 && rr < 256)
- red = (unsigned char) rr;
+ red = (unsigned char)rr;
if (gg >= 0 && gg < 256)
- green = (unsigned char) gg;
+ green = (unsigned char)gg;
if (bb >= 0 && bb < 256)
- blue = (unsigned char) bb;
+ blue = (unsigned char)bb;
}
-
- pcl::OpenNIGrabber grabber (arg);
-
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- OpenNIPassthrough<pcl::PointXYZRGBA> v (grabber, red, green, blue, radius);
- v.run ();
+
+ pcl::OpenNIGrabber grabber(arg);
+
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ OpenNIPassthrough<pcl::PointXYZRGBA> v(grabber, red, green, blue, radius);
+ v.run();
}
- else
- {
+ else {
std::cout << "device does not provide rgb stream" << std::endl;
}
- return (0);
+ return 0;
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/surface/organized_fast_mesh.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
#include <thread>
using namespace std;
using namespace std::chrono_literals;
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- if (++count == 100) \
- { \
- double now = pcl::getTime (); \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ if (++count == 100) { \
+ double now = pcl::getTime(); \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
template <typename PointType>
-class OpenNIFastMesh
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
+class OpenNIFastMesh {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNIFastMesh(const std::string& device_id = "") : device_id_(device_id)
+ {
+ ofm.setTrianglePixelSize(3);
+ ofm.setTriangulationType(pcl::OrganizedFastMesh<PointType>::QUAD_MESH);
+ }
- OpenNIFastMesh (const std::string& device_id = "")
- : device_id_(device_id)
+ void
+ cloud_cb(const CloudConstPtr& cloud)
+ {
+ // Computation goes here
+ FPS_CALC("computation");
+
+ // Prepare input
+ ofm.setInputCloud(cloud);
+
+ // Store the results in a temporary object
+ std::vector<pcl::Vertices> temp_verts;
+ ofm.reconstruct(temp_verts);
+
+ // Lock and copy
{
- ofm.setTrianglePixelSize (3);
- ofm.setTriangulationType (pcl::OrganizedFastMesh<PointType>::QUAD_MESH);
+ std::lock_guard<std::mutex> lock(mtx_);
+ vertices_ = std::move(temp_verts);
+ cloud_ = cloud; // reset (new Cloud (*cloud));
}
-
- void
- cloud_cb (const CloudConstPtr& cloud)
- {
- // Computation goes here
- FPS_CALC ("computation");
-
- // Prepare input
- ofm.setInputCloud (cloud);
-
- // Store the results in a temporary object
- std::vector<pcl::Vertices> temp_verts;
- ofm.reconstruct (temp_verts);
-
- // Lock and copy
- {
- std::lock_guard<std::mutex> lock (mtx_);
- vertices_ = std::move (temp_verts);
- cloud_ = cloud;//reset (new Cloud (*cloud));
+ }
+
+ void
+ run(int argc, char** argv)
+ {
+ pcl::OpenNIGrabber interface{device_id_};
+
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
+
+ view.reset(
+ new pcl::visualization::PCLVisualizer(argc, argv, "PCL OpenNI Mesh Viewer"));
+
+ interface.start();
+
+ CloudConstPtr temp_cloud;
+ std::vector<pcl::Vertices> temp_verts;
+
+ while (!view->wasStopped()) {
+ if (!cloud_ || !mtx_.try_lock()) {
+ std::this_thread::sleep_for(1ms);
+ continue;
}
- }
- void
- run (int argc, char **argv)
- {
- pcl::OpenNIGrabber interface {device_id_};
-
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
-
- view.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCL OpenNI Mesh Viewer"));
-
- interface.start ();
-
- CloudConstPtr temp_cloud;
- std::vector<pcl::Vertices> temp_verts;
-
- while (!view->wasStopped ())
- {
- if (!cloud_ || !mtx_.try_lock ())
- {
- std::this_thread::sleep_for(1ms);
- continue;
- }
-
- temp_cloud = cloud_;
- temp_verts = std::move (vertices_);
- mtx_.unlock ();
-
- if (!view->updatePolygonMesh<PointType> (temp_cloud, temp_verts, "surface"))
- {
- view->addPolygonMesh<PointType> (temp_cloud, temp_verts, "surface");
- view->resetCameraViewpoint ("surface");
- }
-
- FPS_CALC ("visualization");
- view->spinOnce (1);
+ temp_cloud = cloud_;
+ temp_verts = std::move(vertices_);
+ mtx_.unlock();
+
+ if (!view->updatePolygonMesh<PointType>(temp_cloud, temp_verts, "surface")) {
+ view->addPolygonMesh<PointType>(temp_cloud, temp_verts, "surface");
+ view->resetCameraViewpoint("surface");
}
- interface.stop ();
+ FPS_CALC("visualization");
+ view->spinOnce(1);
}
- pcl::OrganizedFastMesh<PointType> ofm;
- std::string device_id_;
- std::mutex mtx_;
- // Data
- CloudConstPtr cloud_;
- std::vector<pcl::Vertices> vertices_;
- pcl::PolygonMesh::Ptr mesh_;
+ interface.stop();
+ }
+
+ pcl::OrganizedFastMesh<PointType> ofm;
+ std::string device_id_;
+ std::mutex mtx_;
+ // Data
+ CloudConstPtr cloud_;
+ std::vector<pcl::Vertices> vertices_;
+ pcl::PolygonMesh::Ptr mesh_;
- pcl::visualization::PCLVisualizer::Ptr view;
+ pcl::visualization::PCLVisualizer::Ptr view;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
- std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
- << ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
- std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
- << " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
- << " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ std::cout << "Device: " << deviceIdx + 1
+ << ", vendor: " << driver.getVendorName(deviceIdx)
+ << ", product: " << driver.getProductName(deviceIdx)
+ << ", connected: " << driver.getBus(deviceIdx) << " @ "
+ << driver.getAddress(deviceIdx) << ", serial number: \'"
+ << driver.getSerialNumber(deviceIdx) << "\'" << std::endl;
+ std::cout << "device_id may be #1, #2, ... for the first second etc device in "
+ "the list or"
+ << std::endl
+ << " bus@address for the device connected to a "
+ "specific usb-bus / address combination (works only in Linux) or"
+ << std::endl
+ << " <serial-number> (only in Linux and for devices "
+ "which provide serial numbers)"
+ << std::endl;
}
}
else
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
std::string arg;
if (argc > 1)
- arg = std::string (argv[1]);
-
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ arg = std::string(argv[1]);
+
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
- pcl::OpenNIGrabber grabber ("");
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- PCL_INFO ("PointXYZRGBA mode enabled.\n");
- OpenNIFastMesh<pcl::PointXYZRGBA> v ("");
- v.run (argc, argv);
+ pcl::OpenNIGrabber grabber("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ PCL_INFO("PointXYZRGBA mode enabled.\n");
+ OpenNIFastMesh<pcl::PointXYZRGBA> v("");
+ v.run(argc, argv);
}
- else
- {
- PCL_INFO ("PointXYZ mode enabled.\n");
- OpenNIFastMesh<pcl::PointXYZ> v ("");
- v.run (argc, argv);
+ else {
+ PCL_INFO("PointXYZ mode enabled.\n");
+ OpenNIFastMesh<pcl::PointXYZ> v("");
+ v.run(argc, argv);
}
- return (0);
+ return 0;
}
*
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/console/parse.h>
#include <pcl/common/time.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/features/multiscale_feature_persistence.h>
+#include <pcl/console/parse.h>
#include <pcl/features/fpfh_omp.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/extract_indices.h>
+#include <pcl/features/multiscale_feature_persistence.h>
#include <pcl/features/normal_3d_omp.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/openni_camera/openni_driver.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
#include <thread>
using namespace std::chrono_literals;
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
const float default_subsampling_leaf_size = 0.02f;
const float default_normal_search_radius = 0.041f;
-const double aux [] = {0.21, 0.32};
-const std::vector<double> default_scales_vector (aux, aux + 2);
+const double aux[] = {0.21, 0.32};
+const std::vector<double> default_scales_vector(aux, aux + 2);
const float default_alpha = 1.2f;
template <typename PointType>
-class OpenNIFeaturePersistence
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNIFeaturePersistence (float &subsampling_leaf_size,
- double &normal_search_radius,
- std::vector<float> &scales_vector,
- float &alpha,
- const std::string& device_id = "")
- : viewer ("PCL OpenNI Feature Persistence Viewer")
- , device_id_(device_id)
- {
- std::cout << "Launching with parameters:\n"
- << " octree_leaf_size = " << subsampling_leaf_size << "\n"
- << " normal_search_radius = " << normal_search_radius << "\n"
- << " persistence_alpha = " << alpha << "\n"
- << " scales = "; for (const float scale : scales_vector) std::cout << scale << " ";
- std::cout << "\n";
-
- subsampling_filter_.setLeafSize (subsampling_leaf_size, subsampling_leaf_size, subsampling_leaf_size);
- typename pcl::search::KdTree<PointType>::Ptr normal_search_tree (new typename pcl::search::KdTree<PointType>);
- normal_estimation_filter_.setSearchMethod (normal_search_tree);
- normal_estimation_filter_.setRadiusSearch (normal_search_radius);
-
- feature_persistence_.setScalesVector (scales_vector);
- feature_persistence_.setAlpha (alpha);
-
- fpfh_estimation_.reset (new typename pcl::FPFHEstimationOMP<PointType, pcl::Normal, pcl::FPFHSignature33> ());
- typename pcl::search::KdTree<PointType>::Ptr fpfh_tree (new typename pcl::search::KdTree<PointType> ());
- fpfh_estimation_->setSearchMethod (fpfh_tree);
- feature_persistence_.setFeatureEstimator (fpfh_estimation_);
- feature_persistence_.setDistanceMetric (pcl::CS);
-
- new_cloud_ = false;
- }
+class OpenNIFeaturePersistence {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNIFeaturePersistence(float& subsampling_leaf_size,
+ double& normal_search_radius,
+ std::vector<float>& scales_vector,
+ float& alpha,
+ const std::string& device_id = "")
+ : viewer("PCL OpenNI Feature Persistence Viewer"), device_id_(device_id)
+ {
+ std::cout << "Launching with parameters:\n"
+ << " octree_leaf_size = " << subsampling_leaf_size << "\n"
+ << " normal_search_radius = " << normal_search_radius << "\n"
+ << " persistence_alpha = " << alpha << "\n"
+ << " scales = ";
+ for (const float scale : scales_vector)
+ std::cout << scale << " ";
+ std::cout << "\n";
+
+ subsampling_filter_.setLeafSize(
+ subsampling_leaf_size, subsampling_leaf_size, subsampling_leaf_size);
+ typename pcl::search::KdTree<PointType>::Ptr normal_search_tree(
+ new typename pcl::search::KdTree<PointType>);
+ normal_estimation_filter_.setSearchMethod(normal_search_tree);
+ normal_estimation_filter_.setRadiusSearch(normal_search_radius);
+
+ feature_persistence_.setScalesVector(scales_vector);
+ feature_persistence_.setAlpha(alpha);
+
+ fpfh_estimation_.reset(new typename pcl::FPFHEstimationOMP<PointType,
+ pcl::Normal,
+ pcl::FPFHSignature33>());
+ typename pcl::search::KdTree<PointType>::Ptr fpfh_tree(
+ new typename pcl::search::KdTree<PointType>());
+ fpfh_estimation_->setSearchMethod(fpfh_tree);
+ feature_persistence_.setFeatureEstimator(fpfh_estimation_);
+ feature_persistence_.setDistanceMetric(pcl::CS);
+
+ new_cloud_ = false;
+ }
+ void
+ cloud_cb(const CloudConstPtr& cloud)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ // lock while we set our cloud;
+ FPS_CALC("computation");
- void
- cloud_cb (const CloudConstPtr& cloud)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- //lock while we set our cloud;
- FPS_CALC ("computation");
+ // Create temporary clouds
+ cloud_subsampled_.reset(new typename pcl::PointCloud<PointType>());
+ normals_.reset(new pcl::PointCloud<pcl::Normal>());
+ features_.reset(new pcl::PointCloud<pcl::FPFHSignature33>());
+ feature_indices_.reset(new std::vector<int>());
+ feature_locations_.reset(new typename pcl::PointCloud<PointType>());
- // Create temporary clouds
- cloud_subsampled_.reset (new typename pcl::PointCloud<PointType> ());
- normals_.reset (new pcl::PointCloud<pcl::Normal> ());
- features_.reset (new pcl::PointCloud<pcl::FPFHSignature33> ());
- feature_indices_.reset (new std::vector<int> ());
- feature_locations_.reset (new typename pcl::PointCloud<PointType> ());
+ // Subsample input cloud
+ subsampling_filter_.setInputCloud(cloud);
+ subsampling_filter_.filter(*cloud_subsampled_);
- // Subsample input cloud
- subsampling_filter_.setInputCloud (cloud);
- subsampling_filter_.filter (*cloud_subsampled_);
+ // Estimate normals
+ normal_estimation_filter_.setInputCloud(cloud_subsampled_);
+ normal_estimation_filter_.compute(*normals_);
- // Estimate normals
- normal_estimation_filter_.setInputCloud (cloud_subsampled_);
- normal_estimation_filter_.compute (*normals_);
+ // Compute persistent features
+ fpfh_estimation_->setInputCloud(cloud_subsampled_);
+ fpfh_estimation_->setInputNormals(normals_);
+ feature_persistence_.determinePersistentFeatures(*features_, feature_indices_);
- // Compute persistent features
- fpfh_estimation_->setInputCloud (cloud_subsampled_);
- fpfh_estimation_->setInputNormals (normals_);
- feature_persistence_.determinePersistentFeatures (*features_, feature_indices_);
+ // Extract feature locations by using indices
+ extract_indices_filter_.setInputCloud(cloud_subsampled_);
+ extract_indices_filter_.setIndices(feature_indices_);
+ extract_indices_filter_.filter(*feature_locations_);
- // Extract feature locations by using indices
- extract_indices_filter_.setInputCloud (cloud_subsampled_);
- extract_indices_filter_.setIndices (feature_indices_);
- extract_indices_filter_.filter (*feature_locations_);
+ PCL_INFO("Persistent feature locations %d\n", feature_locations_->points.size());
- PCL_INFO ("Persistent feature locations %d\n", feature_locations_->points.size ());
+ cloud_ = cloud;
- cloud_ = cloud;
+ new_cloud_ = true;
+ }
- new_cloud_ = true;
+ void
+ viz_cb(pcl::visualization::PCLVisualizer& viz)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ if (!cloud_) {
+ std::this_thread::sleep_for(1s);
+ return;
}
- void
- viz_cb (pcl::visualization::PCLVisualizer& viz)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- if (!cloud_)
- {
- std::this_thread::sleep_for(1s);
- return;
- }
-
- CloudConstPtr temp_cloud;
- temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
-
-// if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
-// {
-// viz.addPointCloud (temp_cloud, "OpenNICloud");
-// viz.resetCameraViewpoint ("OpenNICloud");
-// }
- // Render the data
- if (new_cloud_ && feature_locations_)
- {
- viz.removePointCloud ("featurecloud");
- viz.removePointCloud ("OpenNICloud");
- colors_.reset (new typename pcl::visualization::PointCloudColorHandlerCustom<PointType> (feature_locations_, 255.0, 0.0, 0.0));
- viz.addPointCloud (feature_locations_, *colors_, "featurecloud");
- viz.addPointCloud (temp_cloud, "OpenNICloud");
- new_cloud_ = false;
- }
+ CloudConstPtr temp_cloud;
+ temp_cloud.swap(cloud_); // here we set cloud_ to null, so that
+
+ // Render the data
+ if (new_cloud_ && feature_locations_) {
+ viz.removePointCloud("featurecloud");
+ viz.removePointCloud("OpenNICloud");
+ colors_.reset(
+ new typename pcl::visualization::PointCloudColorHandlerCustom<PointType>(
+ feature_locations_, 255.0, 0.0, 0.0));
+ viz.addPointCloud(feature_locations_, *colors_, "featurecloud");
+ viz.addPointCloud(temp_cloud, "OpenNICloud");
+ new_cloud_ = false;
}
+ }
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
-
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
- viewer.runOnVisualizationThread ([this] (pcl::visualization::PCLVisualizer& viz) { viz_cb (viz); }, "viz_cb");
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- interface.start ();
+ viewer.runOnVisualizationThread(
+ [this](pcl::visualization::PCLVisualizer& viz) { viz_cb(viz); }, "viz_cb");
- while (!viewer.wasStopped ())
- {
- std::this_thread::sleep_for(1s);
- }
+ interface.start();
- interface.stop ();
+ while (!viewer.wasStopped()) {
+ std::this_thread::sleep_for(1s);
}
+ interface.stop();
+ }
- pcl::VoxelGrid<PointType> subsampling_filter_;
- pcl::NormalEstimationOMP<PointType, pcl::Normal> normal_estimation_filter_;
- typename pcl::FPFHEstimationOMP<PointType, pcl::Normal, pcl::FPFHSignature33>::Ptr fpfh_estimation_;
- pcl::MultiscaleFeaturePersistence<PointType, pcl::FPFHSignature33> feature_persistence_;
- pcl::ExtractIndices<PointType> extract_indices_filter_;
-
- pcl::visualization::CloudViewer viewer;
- std::string device_id_;
- std::mutex mtx_;
- // Data
- CloudPtr feature_locations_, cloud_subsampled_;
- pcl::PointCloud<pcl::Normal>::Ptr normals_;
- pcl::PointCloud<pcl::FPFHSignature33>::Ptr features_;
- typename pcl::visualization::PointCloudColorHandlerCustom<PointType>::Ptr colors_;
- pcl::IndicesPtr feature_indices_;
- CloudConstPtr cloud_;
- bool new_cloud_;
+ pcl::VoxelGrid<PointType> subsampling_filter_;
+ pcl::NormalEstimationOMP<PointType, pcl::Normal> normal_estimation_filter_;
+ typename pcl::FPFHEstimationOMP<PointType, pcl::Normal, pcl::FPFHSignature33>::Ptr
+ fpfh_estimation_;
+ pcl::MultiscaleFeaturePersistence<PointType, pcl::FPFHSignature33>
+ feature_persistence_;
+ pcl::ExtractIndices<PointType> extract_indices_filter_;
+
+ pcl::visualization::CloudViewer viewer;
+ std::string device_id_;
+ std::mutex mtx_;
+ // Data
+ CloudPtr feature_locations_, cloud_subsampled_;
+ pcl::PointCloud<pcl::Normal>::Ptr normals_;
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr features_;
+ typename pcl::visualization::PointCloudColorHandlerCustom<PointType>::Ptr colors_;
+ pcl::IndicesPtr feature_indices_;
+ CloudConstPtr cloud_;
+ bool new_cloud_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
+ // clang-format off
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n"
<< "where options are:\n"
<< " -octree_leaf_size X = size of the leaf for the octree-based subsampling filter (default: " << default_subsampling_leaf_size << "\n"
<< " -normal_search_radius X = size of the neighborhood to consider for calculating the local plane and extracting the normals (default: " << default_normal_search_radius << "\n"
<< " -persistence_alpha X = value of alpha for the multiscale feature persistence (default: " << default_alpha << "\n"
<< " -scales X1 X2 ... = values for the multiple scales for extracting features (default: ";
- for (const double &i : default_scales_vector) std::cout << i << " ";
+ // clang-format on
+ for (const double& i : default_scales_vector)
+ std::cout << i << " ";
std::cout << "\n\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
}
int
-main (int argc, char **argv)
+main(int argc, char** argv)
{
- std::cout << "OpenNIFeaturePersistence - show persistent features based on the MultiscaleFeaturePersistence class using the FPFH features\n"
+ std::cout << "OpenNIFeaturePersistence - show persistent features based on the "
+ "MultiscaleFeaturePersistence class using the FPFH features\n"
<< "Use \"-h\" to get more info about the available options.\n";
- if (pcl::console::find_argument (argc, argv, "-h") == -1)
- {
- usage (argv);
+ if (pcl::console::find_argument(argc, argv, "-h") == -1) {
+ usage(argv);
return 1;
}
// Parse arguments
float subsampling_leaf_size = default_subsampling_leaf_size;
- pcl::console::parse_argument (argc, argv, "-octree_leaf_size", subsampling_leaf_size);
+ pcl::console::parse_argument(argc, argv, "-octree_leaf_size", subsampling_leaf_size);
double normal_search_radius = default_normal_search_radius;
- pcl::console::parse_argument (argc, argv, "-normal_search_radius", normal_search_radius);
+ pcl::console::parse_argument(
+ argc, argv, "-normal_search_radius", normal_search_radius);
std::vector<double> scales_vector_double = default_scales_vector;
- pcl::console::parse_multiple_arguments (argc, argv, "-scales", scales_vector_double);
- std::vector<float> scales_vector (scales_vector_double.size ());
- for (std::size_t i = 0; i < scales_vector_double.size (); ++i) scales_vector[i] = float (scales_vector_double[i]);
+ pcl::console::parse_multiple_arguments(argc, argv, "-scales", scales_vector_double);
+ std::vector<float> scales_vector(scales_vector_double.size());
+ for (std::size_t i = 0; i < scales_vector_double.size(); ++i)
+ scales_vector[i] = float(scales_vector_double[i]);
float alpha = default_alpha;
- pcl::console::parse_argument (argc, argv, "-persistence_alpha", alpha);
-
-
- pcl::OpenNIGrabber grabber ("");
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- PCL_INFO ("PointXYZRGBA mode enabled.\n");
- OpenNIFeaturePersistence<pcl::PointXYZRGBA> v (subsampling_leaf_size,
- normal_search_radius,
- scales_vector,
- alpha,
- "");
- v.run ();
+ pcl::console::parse_argument(argc, argv, "-persistence_alpha", alpha);
+
+ pcl::OpenNIGrabber grabber("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ PCL_INFO("PointXYZRGBA mode enabled.\n");
+ OpenNIFeaturePersistence<pcl::PointXYZRGBA> v(
+ subsampling_leaf_size, normal_search_radius, scales_vector, alpha, "");
+ v.run();
}
- else
- {
- PCL_INFO ("PointXYZ mode enabled.\n");
- OpenNIFeaturePersistence<pcl::PointXYZ> v (subsampling_leaf_size,
- normal_search_radius,
- scales_vector,
- alpha,
- "");
- v.run ();
+ else {
+ PCL_INFO("PointXYZ mode enabled.\n");
+ OpenNIFeaturePersistence<pcl::PointXYZ> v(
+ subsampling_leaf_size, normal_search_radius, scales_vector, alpha, "");
+ v.run();
}
- return (0);
+ return 0;
}
-
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
* Author: Nico Blodow (blodow@cs.tum.edu)
* Christian Potthast (potthast@usc.edu)
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h>
-#include <pcl/console/print.h>
#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <boost/filesystem.hpp>
#define SHOW_FPS 1
#if SHOW_FPS
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
#else
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
-}while(false)
+#define FPS_CALC(_WHAT_) \
+ do { \
+ } while (false)
#endif
using namespace pcl::console;
using namespace boost::filesystem;
-template<typename PointType>
-class OpenNIGrabFrame
-{
+template <typename PointType>
+class OpenNIGrabFrame {
using Cloud = pcl::PointCloud<PointType>;
- using CloudConstPtr = typename Cloud::ConstPtr;
- public:
- OpenNIGrabFrame (pcl::OpenNIGrabber &grabber)
- : visualizer_ (new pcl::visualization::PCLVisualizer ("OpenNI Viewer"))
- , writer_ ()
- , quit_ (false)
- , continuous_ (false)
- , trigger_ (false)
- , file_name_ ("")
- , dir_name_ ("")
- , format_ (4)
- , grabber_ (grabber)
- , visualizer_enable_ (true)
- {
- }
+ using CloudConstPtr = typename Cloud::ConstPtr;
- void
- cloud_cb_ (const CloudConstPtr& cloud)
- {
- if (quit_)
- return;
-
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- cloud_ = cloud;
-
- if (continuous_ || trigger_)
- saveCloud ();
-
- trigger_ = false;
- }
-
- void
- keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
- {
- if (event.keyUp ())
- {
- switch (event.getKeyCode ())
- {
- case 27:
- case 'Q':
- case 'q': quit_ = true; visualizer_->close ();
- break;
- case 'V':
- case 'v': visualizer_enable_ = !visualizer_enable_;
- break;
- case ' ': continuous_ = !continuous_;
- break;
- }
+public:
+ OpenNIGrabFrame(pcl::OpenNIGrabber& grabber)
+ : visualizer_(new pcl::visualization::PCLVisualizer("OpenNI Viewer"))
+ , writer_()
+ , quit_(false)
+ , continuous_(false)
+ , trigger_(false)
+ , file_name_("")
+ , dir_name_("")
+ , format_(4)
+ , grabber_(grabber)
+ , visualizer_enable_(true)
+ {}
+
+ void
+ cloud_cb_(const CloudConstPtr& cloud)
+ {
+ if (quit_)
+ return;
+
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ cloud_ = cloud;
+
+ if (continuous_ || trigger_)
+ saveCloud();
+
+ trigger_ = false;
+ }
+
+ void
+ keyboard_callback(const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ if (event.keyUp()) {
+ switch (event.getKeyCode()) {
+ case 27:
+ case 'Q':
+ case 'q':
+ quit_ = true;
+ visualizer_->close();
+ break;
+ case 'V':
+ case 'v':
+ visualizer_enable_ = !visualizer_enable_;
+ break;
+ case ' ':
+ continuous_ = !continuous_;
+ break;
}
}
-
- void
- mouse_callback (const pcl::visualization::MouseEvent& mouse_event, void*)
- {
- if (mouse_event.getType() == pcl::visualization::MouseEvent::MouseButtonPress && mouse_event.getButton() == pcl::visualization::MouseEvent::LeftButton)
- {
- trigger_ = true;
- }
+ }
+
+ void
+ mouse_callback(const pcl::visualization::MouseEvent& mouse_event, void*)
+ {
+ if (mouse_event.getType() == pcl::visualization::MouseEvent::MouseButtonPress &&
+ mouse_event.getButton() == pcl::visualization::MouseEvent::LeftButton) {
+ trigger_ = true;
}
-
- CloudConstPtr
- getLatestCloud ()
- {
- //lock while we swap our cloud and reset it.
- std::lock_guard<std::mutex> lock(cloud_mutex_);
- CloudConstPtr temp_cloud;
- temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
- //it is safe to set it again from our
- //callback
- return (temp_cloud);
+ }
+
+ CloudConstPtr
+ getLatestCloud()
+ {
+ // lock while we swap our cloud and reset it.
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ CloudConstPtr temp_cloud;
+ temp_cloud.swap(cloud_); // here we set cloud_ to null, so that
+ // it is safe to set it again from our
+ // callback
+ return temp_cloud;
+ }
+
+ void
+ saveCloud()
+ {
+ FPS_CALC("I/O");
+ std::stringstream ss;
+ ss << dir_name_ << "/" << file_name_ << "_"
+ << boost::posix_time::to_iso_string(
+ boost::posix_time::microsec_clock::local_time())
+ << ".pcd";
+
+ if (format_ & 1) {
+ writer_.writeBinary<PointType>(ss.str(), *cloud_);
+ // std::cerr << "Data saved in BINARY format to " << ss.str () << std::endl;
}
-
- void saveCloud ()
- {
- FPS_CALC ("I/O");
- std::stringstream ss;
- ss << dir_name_ << "/" << file_name_ << "_" << boost::posix_time::to_iso_string (boost::posix_time::microsec_clock::local_time ()) << ".pcd";
-
- if (format_ & 1)
- {
- writer_.writeBinary<PointType> (ss.str (), *cloud_);
- //std::cerr << "Data saved in BINARY format to " << ss.str () << std::endl;
- }
-
- if (format_ & 2)
- {
- writer_.writeBinaryCompressed<PointType> (ss.str (), *cloud_);
- //std::cerr << "Data saved in BINARY COMPRESSED format to " << ss.str () << std::endl;
- }
-
- if (format_ & 4)
- {
- writer_.writeBinaryCompressed<PointType> (ss.str (), *cloud_);
- //std::cerr << "Data saved in BINARY COMPRESSED format to " << ss.str () << std::endl;
- }
+
+ if (format_ & 2) {
+ writer_.writeBinaryCompressed<PointType>(ss.str(), *cloud_);
+ }
+
+ if (format_ & 4) {
+ writer_.writeBinaryCompressed<PointType>(ss.str(), *cloud_);
}
-
- void
- run ()
- {
- // register the keyboard and mouse callback for the visualizer
- visualizer_->registerMouseCallback (&OpenNIGrabFrame::mouse_callback, *this);
- visualizer_->registerKeyboardCallback(&OpenNIGrabFrame::keyboard_callback, *this);
-
-
- // make callback function from member function
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb_ (cloud); };
-
- // connect callback function for desired signal. In this case its a point cloud with color values
- boost::signals2::connection c = grabber_.registerCallback (f);
-
- // start receiving point clouds
- grabber_.start ();
-
- // wait until user quits program with Ctrl-C, but no busy-waiting -> sleep (1);
- while (!visualizer_->wasStopped())
- {
- std::this_thread::sleep_for(100us);
-
- visualizer_->spinOnce ();
-
- if (!visualizer_enable_)
- continue;
-
- if (cloud_)
- {
- CloudConstPtr cloud = getLatestCloud ();
- if (!visualizer_->updatePointCloud (cloud, "OpenNICloud"))
- {
- visualizer_->addPointCloud (cloud, "OpenNICloud");
- visualizer_->resetCameraViewpoint ("OpenNICloud");
- }
+ }
+
+ void
+ run()
+ {
+ // register the keyboard and mouse callback for the visualizer
+ visualizer_->registerMouseCallback(&OpenNIGrabFrame::mouse_callback, *this);
+ visualizer_->registerKeyboardCallback(&OpenNIGrabFrame::keyboard_callback, *this);
+
+ // make callback function from member function
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+
+ // connect callback function for desired signal. In this case its a point cloud with
+ // color values
+ boost::signals2::connection c = grabber_.registerCallback(f);
+
+ // start receiving point clouds
+ grabber_.start();
+
+ // wait until user quits program with Ctrl-C, but no busy-waiting -> sleep (1);
+ while (!visualizer_->wasStopped()) {
+ std::this_thread::sleep_for(100us);
+
+ visualizer_->spinOnce();
+
+ if (!visualizer_enable_)
+ continue;
+
+ if (cloud_) {
+ CloudConstPtr cloud = getLatestCloud();
+ if (!visualizer_->updatePointCloud(cloud, "OpenNICloud")) {
+ visualizer_->addPointCloud(cloud, "OpenNICloud");
+ visualizer_->resetCameraViewpoint("OpenNICloud");
}
}
-
- // stop the grabber
- grabber_.stop ();
}
- void
- setOptions (const std::string &filename, const std::string &pcd_format, bool paused, bool visualizer)
- {
- boost::filesystem::path path(filename);
+ // stop the grabber
+ grabber_.stop();
+ }
- if (filename.empty ())
- {
- dir_name_ = ".";
- file_name_ = "frame";
- }
- else
- {
- dir_name_ = path.parent_path ().string ();
-
- if (!dir_name_.empty () && !boost::filesystem::exists (path.parent_path ()))
- {
- std::cerr << "directory \"" << path.parent_path () << "\" does not exist!\n";
- exit (1);
- }
- file_name_ = path.stem ().string ();
+ void
+ setOptions(const std::string& filename,
+ const std::string& pcd_format,
+ bool paused,
+ bool visualizer)
+ {
+ boost::filesystem::path path(filename);
+
+ if (filename.empty()) {
+ dir_name_ = ".";
+ file_name_ = "frame";
+ }
+ else {
+ dir_name_ = path.parent_path().string();
+
+ if (!dir_name_.empty() && !boost::filesystem::exists(path.parent_path())) {
+ std::cerr << "directory \"" << path.parent_path() << "\" does not exist!\n";
+ exit(1);
}
-
- std::cout << "dir: " << dir_name_ << " :: " << path.parent_path () << std::endl;
- std::cout << "file: " << file_name_ << " :: " << path.stem (). string () << std::endl;
-
- if (pcd_format == "b" || pcd_format == "all")
- format_ |= 1;
- else if (pcd_format == "ascii" || pcd_format == "all")
- format_ |= 2;
- else if (pcd_format == "bc" || pcd_format == "all")
- format_ |= 4;
-
- continuous_ = !paused;
- visualizer_enable_ = visualizer;
+ file_name_ = path.stem().string();
}
- pcl::visualization::PCLVisualizer::Ptr visualizer_;
- pcl::PCDWriter writer_;
- bool quit_;
- bool continuous_;
- bool trigger_;
- std::string file_name_;
- std::string dir_name_;
- unsigned format_;
- CloudConstPtr cloud_;
- mutable std::mutex cloud_mutex_;
- pcl::OpenNIGrabber &grabber_;
- bool visualizer_enable_;
+ std::cout << "dir: " << dir_name_ << " :: " << path.parent_path() << std::endl;
+ std::cout << "file: " << file_name_ << " :: " << path.stem().string() << std::endl;
+
+ if (pcd_format == "b" || pcd_format == "all")
+ format_ |= 1;
+ else if (pcd_format == "ascii" || pcd_format == "all")
+ format_ |= 2;
+ else if (pcd_format == "bc" || pcd_format == "all")
+ format_ |= 4;
+
+ continuous_ = !paused;
+ visualizer_enable_ = visualizer;
+ }
+
+ pcl::visualization::PCLVisualizer::Ptr visualizer_;
+ pcl::PCDWriter writer_;
+ bool quit_;
+ bool continuous_;
+ bool trigger_;
+ std::string file_name_;
+ std::string dir_name_;
+ unsigned format_;
+ CloudConstPtr cloud_;
+ mutable std::mutex cloud_mutex_;
+ pcl::OpenNIGrabber& grabber_;
+ bool visualizer_enable_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <filename> <options>\n\n";
-
+
+ // clang-format off
print_info (" filename: if no filename is provided a generic timestamp will be set as filename\n\n");
print_info (" where options are:\n");
print_info (" -format = PCD file format (b=binary; bc=binary compressed; ascii=ascii; all=all) (default: bc)\n");
print_info (" -imagemode = select the image mode (resolution, fps) for the grabber, see pcl::OpenNIGrabber::Mode for details.\n");
print_info (" -depthmode = select the depth mode (resolution, fps) for the grabber, see pcl::OpenNIGrabber::Mode for details.\n");
print_info (" -visualizer 0/1 = turn OFF or ON the visualization of point clouds in the viewer (can also be changed using 'v'/'V' in the viewer).\n");
+ // clang-format on
}
-int
-main (int argc, char** argv)
+int
+main(int argc, char** argv)
{
std::string arg;
if (argc > 1)
- arg = std::string (argv[1]);
+ arg = std::string(argv[1]);
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
pcl::OpenNIGrabber::Mode depth_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode;
pcl::OpenNIGrabber::Mode image_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode;
- if (argc > 1)
- {
+ if (argc > 1) {
// Parse the command line arguments for .pcd file
std::vector<int> p_file_indices;
- p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
- if (p_file_indices.size () > 0)
+ p_file_indices = parse_file_extension_argument(argc, argv, ".pcd");
+ if (p_file_indices.size() > 0)
filename = argv[p_file_indices[0]];
-
+
std::cout << "fname: " << filename << std::endl;
// Command line parsing
- parse_argument (argc, argv, "-format", format);
- xyz = find_switch (argc, argv, "-XYZ");
- paused = find_switch (argc, argv, "-paused");
- visualizer = find_switch (argc, argv, "-visualizer");
+ parse_argument(argc, argv, "-format", format);
+ xyz = find_switch(argc, argv, "-XYZ");
+ paused = find_switch(argc, argv, "-paused");
+ visualizer = find_switch(argc, argv, "-visualizer");
unsigned mode;
if (pcl::console::parse(argc, argv, "-depthmode", mode) != -1)
- depth_mode = pcl::OpenNIGrabber::Mode (mode);
+ depth_mode = pcl::OpenNIGrabber::Mode(mode);
if (pcl::console::parse(argc, argv, "-imagemode", mode) != -1)
- image_mode = pcl::OpenNIGrabber::Mode (mode);
+ image_mode = pcl::OpenNIGrabber::Mode(mode);
}
- pcl::OpenNIGrabber grabber ("#1", depth_mode, image_mode);
+ pcl::OpenNIGrabber grabber("#1", depth_mode, image_mode);
- if (xyz)
- {
- OpenNIGrabFrame<pcl::PointXYZ> grab_frame (grabber);
- grab_frame.setOptions (filename, format, paused, visualizer);
- grab_frame.run ();
+ if (xyz) {
+ OpenNIGrabFrame<pcl::PointXYZ> grab_frame(grabber);
+ grab_frame.setOptions(filename, format, paused, visualizer);
+ grab_frame.run();
}
- else
- {
- OpenNIGrabFrame<pcl::PointXYZRGBA> grab_frame (grabber);
- grab_frame.setOptions (filename, format, paused, visualizer);
- grab_frame.run ();
+ else {
+ OpenNIGrabFrame<pcl::PointXYZRGBA> grab_frame(grabber);
+ grab_frame.setOptions(filename, format, paused, visualizer);
+ grab_frame.run();
}
- return (0);
+ return 0;
}
-
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
* Author: Nico Blodow (blodow@cs.tum.edu)
* Christian Potthast (potthast@usc.edu)
*/
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h>
-#include <pcl/console/print.h>
#include <pcl/console/parse.h>
-#include <pcl/visualization/vtk.h>
-#include <pcl/visualization/image_viewer.h>
+#include <pcl/console/print.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/visualization/common/float_image_utils.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/visualization/vtk.h>
+#include <pcl/point_types.h>
#include <boost/filesystem.hpp>
using namespace pcl::console;
using namespace boost::filesystem;
-class OpenNIGrabFrame
-{
- public:
- OpenNIGrabFrame (pcl::OpenNIGrabber& grabber, bool paused)
- : grabber_ (grabber)
- , image_viewer_ ("RGB Image")
- , depth_image_viewer_ ("Depth Image")
- , quit_ (false)
- , continuous_ (!paused)
- , trigger_ (false)
- , importer_ (vtkSmartPointer<vtkImageImport>::New ())
- , depth_importer_ (vtkSmartPointer<vtkImageImport>::New ())
- , writer_ (vtkSmartPointer<vtkTIFFWriter>::New ())
- , flipper_ (vtkSmartPointer<vtkImageFlip>::New ())
- {
- importer_->SetNumberOfScalarComponents (3);
- importer_->SetDataScalarTypeToUnsignedChar ();
- depth_importer_->SetNumberOfScalarComponents (1);
- depth_importer_->SetDataScalarTypeToUnsignedShort ();
- writer_->SetCompressionToPackBits ();
- flipper_->SetFilteredAxes (1);
+class OpenNIGrabFrame {
+public:
+ OpenNIGrabFrame(pcl::OpenNIGrabber& grabber, bool paused)
+ : grabber_(grabber)
+ , image_viewer_("RGB Image")
+ , depth_image_viewer_("Depth Image")
+ , quit_(false)
+ , continuous_(!paused)
+ , trigger_(false)
+ , importer_(vtkSmartPointer<vtkImageImport>::New())
+ , depth_importer_(vtkSmartPointer<vtkImageImport>::New())
+ , writer_(vtkSmartPointer<vtkTIFFWriter>::New())
+ , flipper_(vtkSmartPointer<vtkImageFlip>::New())
+ {
+ importer_->SetNumberOfScalarComponents(3);
+ importer_->SetDataScalarTypeToUnsignedChar();
+ depth_importer_->SetNumberOfScalarComponents(1);
+ depth_importer_->SetDataScalarTypeToUnsignedShort();
+ writer_->SetCompressionToPackBits();
+ flipper_->SetFilteredAxes(1);
+ }
+
+ void
+ image_callback(const openni_wrapper::Image::Ptr& image,
+ const openni_wrapper::DepthImage::Ptr& depth_image,
+ float)
+ {
+ std::lock_guard<std::mutex> lock(image_mutex_);
+ image_ = image;
+ depth_image_ = depth_image;
+ lock.unlock();
+ }
+
+ void
+ keyboard_callback(const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ if (event.keyUp()) {
+ switch (event.getKeyCode()) {
+ case 27:
+ case 'Q':
+ case 'q':
+ quit_ = true;
+ break;
+ case ' ':
+ continuous_ = !continuous_;
+ break;
+ case 'G':
+ case 'g':
+ trigger_ = true;
+ break;
+ }
}
+ }
- void
- image_callback (const openni_wrapper::Image::Ptr &image,
- const openni_wrapper::DepthImage::Ptr &depth_image, float)
- {
- std::lock_guard<std::mutex> lock (image_mutex_);
- image_ = image;
- depth_image_ = depth_image;
- lock.unlock ();
+ void
+ mouse_callback(const pcl::visualization::MouseEvent& mouse_event, void*)
+ {
+ if (mouse_event.getType() == pcl::visualization::MouseEvent::MouseButtonPress &&
+ mouse_event.getButton() == pcl::visualization::MouseEvent::LeftButton) {
+ trigger_ = true;
}
-
- void
- keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
- {
- if (event.keyUp ())
- {
- switch (event.getKeyCode ())
- {
- case 27:
- case 'Q':
- case 'q': quit_ = true;
- break;
- case ' ': continuous_ = !continuous_;
- break;
- case 'G':
- case 'g': trigger_ = true;
- break;
+ }
+
+ void
+ saveImages()
+ {
+ std::string time = boost::posix_time::to_iso_string(
+ boost::posix_time::microsec_clock::local_time());
+ openni_wrapper::Image::Ptr image;
+ openni_wrapper::DepthImage::Ptr depth_image;
+
+ image_mutex_.lock();
+ image.swap(image_);
+ depth_image.swap(depth_image_);
+ image_mutex_.unlock();
+
+ if (image) {
+ const void* data;
+ if (image->getEncoding() != openni_wrapper::Image::RGB) {
+ static unsigned char* rgb_data = 0;
+ static unsigned rgb_data_size = 0;
+
+ if (rgb_data_size < image->getWidth() * image->getHeight()) {
+ delete[] rgb_data;
+ rgb_data_size = image->getWidth() * image->getHeight();
+ rgb_data = new unsigned char[rgb_data_size * 3];
}
+ image->fillRGB(image->getWidth(), image->getHeight(), rgb_data);
+ data = reinterpret_cast<const void*>(rgb_data);
}
+ else
+ data = reinterpret_cast<const void*>(image->getMetaData().Data());
+
+ importer_->SetWholeExtent(
+ 0, image->getWidth() - 1, 0, image->getHeight() - 1, 0, 0);
+ importer_->SetDataExtentToWholeExtent();
+
+ std::stringstream ss;
+ ss << "frame_" + time + "_rgb.tiff";
+ importer_->SetImportVoidPointer(const_cast<void*>(data), 1);
+ importer_->Update();
+ flipper_->SetInputConnection(importer_->GetOutputPort());
+ flipper_->Update();
+ writer_->SetFileName(ss.str().c_str());
+ writer_->SetInputConnection(flipper_->GetOutputPort());
+ writer_->Write();
}
-
- void
- mouse_callback (const pcl::visualization::MouseEvent& mouse_event, void*)
- {
- if (mouse_event.getType() == pcl::visualization::MouseEvent::MouseButtonPress && mouse_event.getButton() == pcl::visualization::MouseEvent::LeftButton)
- {
- trigger_ = true;
- }
+ if (depth_image) {
+ std::stringstream ss;
+ ss << "frame_" + time + "_depth.tiff";
+
+ depth_importer_->SetWholeExtent(
+ 0, depth_image->getWidth() - 1, 0, depth_image->getHeight() - 1, 0, 0);
+ depth_importer_->SetDataExtentToWholeExtent();
+ depth_importer_->SetImportVoidPointer(
+ const_cast<void*>(
+ reinterpret_cast<const void*>(depth_image->getDepthMetaData().Data())),
+ 1);
+ depth_importer_->Update();
+ flipper_->SetInputConnection(depth_importer_->GetOutputPort());
+ flipper_->Update();
+ writer_->SetFileName(ss.str().c_str());
+ writer_->SetInputConnection(flipper_->GetOutputPort());
+ writer_->Write();
}
-
- void saveImages ()
- {
- std::string time = boost::posix_time::to_iso_string (boost::posix_time::microsec_clock::local_time ());
+
+ trigger_ = false;
+ }
+
+ int
+ run()
+ {
+ // register the keyboard and mouse callback for the visualizer
+ image_viewer_.registerMouseCallback(&OpenNIGrabFrame::mouse_callback, *this);
+ image_viewer_.registerKeyboardCallback(&OpenNIGrabFrame::keyboard_callback, *this);
+ depth_image_viewer_.registerMouseCallback(&OpenNIGrabFrame::mouse_callback, *this);
+ depth_image_viewer_.registerKeyboardCallback(&OpenNIGrabFrame::keyboard_callback,
+ *this);
+
+ std::function<void(const openni_wrapper::Image::Ptr&,
+ const openni_wrapper::DepthImage::Ptr&,
+ float)>
+ image_cb = [this](const openni_wrapper::Image::Ptr& img,
+ const openni_wrapper::DepthImage::Ptr& depth,
+ float f) { image_callback(img, depth, f); };
+ boost::signals2::connection image_connection = grabber_.registerCallback(image_cb);
+
+ // start receiving point clouds
+ grabber_.start();
+ unsigned char* rgb_data = 0;
+ unsigned rgb_data_size = 0;
+ unsigned char* depth_data = 0;
+
+ // wait until user quits program with Ctrl-C, but no busy-waiting -> sleep (1);
+ while (!image_viewer_.wasStopped() && !quit_) {
+ std::string time = boost::posix_time::to_iso_string(
+ boost::posix_time::microsec_clock::local_time());
openni_wrapper::Image::Ptr image;
openni_wrapper::DepthImage::Ptr depth_image;
- image_mutex_.lock ();
- image.swap (image_);
- depth_image.swap (depth_image_);
- image_mutex_.unlock ();
+ if (image_mutex_.try_lock()) {
+ image.swap(image_);
+ depth_image.swap(depth_image_);
+ image_mutex_.unlock();
+ }
- if (image)
- {
+ if (image) {
const void* data;
- if (image->getEncoding() != openni_wrapper::Image::RGB)
- {
- static unsigned char* rgb_data = 0;
- static unsigned rgb_data_size = 0;
-
- if (rgb_data_size < image->getWidth () * image->getHeight ())
- {
- if (rgb_data)
- delete [] rgb_data;
- rgb_data_size = image->getWidth () * image->getHeight ();
- rgb_data = new unsigned char [rgb_data_size * 3];
+ if (image->getEncoding() != openni_wrapper::Image::RGB) {
+ if (rgb_data_size < image->getWidth() * image->getHeight()) {
+ delete[] rgb_data;
+ rgb_data_size = image->getWidth() * image->getHeight();
+ rgb_data = new unsigned char[rgb_data_size * 3];
}
- image->fillRGB (image->getWidth (), image->getHeight (), rgb_data);
- data = reinterpret_cast<const void*> (rgb_data);
+ image->fillRGB(image->getWidth(), image->getHeight(), rgb_data);
+ data = reinterpret_cast<const void*>(rgb_data);
}
else
- data = reinterpret_cast<const void*> (image->getMetaData ().Data ());
-
- importer_->SetWholeExtent (0, image->getWidth () - 1, 0, image->getHeight () - 1, 0, 0);
- importer_->SetDataExtentToWholeExtent ();
-
- std::stringstream ss;
- ss << "frame_" + time + "_rgb.tiff";
- importer_->SetImportVoidPointer (const_cast<void*>(data), 1);
- importer_->Update ();
- flipper_->SetInputConnection (importer_->GetOutputPort ());
- flipper_->Update ();
- writer_->SetFileName (ss.str ().c_str ());
- writer_->SetInputConnection (flipper_->GetOutputPort ());
- writer_->Write ();
- }
- if (depth_image)
- {
- std::stringstream ss;
- ss << "frame_" + time + "_depth.tiff";
-
- depth_importer_->SetWholeExtent (0, depth_image->getWidth () - 1, 0, depth_image->getHeight () - 1, 0, 0);
- depth_importer_->SetDataExtentToWholeExtent ();
- depth_importer_->SetImportVoidPointer (const_cast<void*> (reinterpret_cast<const void*> (depth_image->getDepthMetaData ().Data ())), 1);
- depth_importer_->Update ();
- flipper_->SetInputConnection (depth_importer_->GetOutputPort ());
- flipper_->Update ();
- writer_->SetFileName (ss.str ().c_str ());
- writer_->SetInputConnection (flipper_->GetOutputPort ());
- writer_->Write ();
- }
+ data = reinterpret_cast<const void*>(image->getMetaData().Data());
+ image_viewer_.addRGBImage(
+ (const unsigned char*)data, image->getWidth(), image->getHeight());
- trigger_ = false;
- }
-
- int
- run ()
- {
- // register the keyboard and mouse callback for the visualizer
- image_viewer_.registerMouseCallback (&OpenNIGrabFrame::mouse_callback, *this);
- image_viewer_.registerKeyboardCallback(&OpenNIGrabFrame::keyboard_callback, *this);
- depth_image_viewer_.registerMouseCallback (&OpenNIGrabFrame::mouse_callback, *this);
- depth_image_viewer_.registerKeyboardCallback(&OpenNIGrabFrame::keyboard_callback, *this);
-
- std::function<void (const openni_wrapper::Image::Ptr&,
- const openni_wrapper::DepthImage::Ptr&,
- float) > image_cb = [this] (const openni_wrapper::Image::Ptr& img,
- const openni_wrapper::DepthImage::Ptr& depth,
- float f)
- {
- image_callback (img, depth, f);
- };
- boost::signals2::connection image_connection = grabber_.registerCallback (image_cb);
-
- // start receiving point clouds
- grabber_.start ();
- unsigned char* rgb_data = 0;
- unsigned rgb_data_size = 0;
- unsigned char* depth_data = 0;
-
- // wait until user quits program with Ctrl-C, but no busy-waiting -> sleep (1);
- while (!image_viewer_.wasStopped() && !quit_)
- {
- std::string time = boost::posix_time::to_iso_string (boost::posix_time::microsec_clock::local_time ());
- openni_wrapper::Image::Ptr image;
- openni_wrapper::DepthImage::Ptr depth_image;
-
- if (image_mutex_.try_lock ())
- {
- image.swap (image_);
- depth_image.swap (depth_image_);
- image_mutex_.unlock ();
+ if (continuous_ || trigger_) {
+ importer_->SetWholeExtent(
+ 0, image->getWidth() - 1, 0, image->getHeight() - 1, 0, 0);
+ importer_->SetDataExtentToWholeExtent();
+
+ std::stringstream ss;
+ ss << "frame_" + time + "_rgb.tiff";
+ importer_->SetImportVoidPointer(const_cast<void*>(data), 1);
+ importer_->Update();
+ flipper_->SetInputConnection(importer_->GetOutputPort());
+ flipper_->Update();
+ writer_->SetFileName(ss.str().c_str());
+ writer_->SetInputConnection(flipper_->GetOutputPort());
+ writer_->Write();
+ std::cout << "writing rgb frame: " << ss.str() << std::endl;
}
-
- if (image)
- {
- const void* data;
- if (image->getEncoding() != openni_wrapper::Image::RGB)
- {
- if (rgb_data_size < image->getWidth () * image->getHeight ())
- {
- if (rgb_data)
- delete [] rgb_data;
- rgb_data_size = image->getWidth () * image->getHeight ();
- rgb_data = new unsigned char [rgb_data_size * 3];
- }
- image->fillRGB (image->getWidth (), image->getHeight (), rgb_data);
- data = reinterpret_cast<const void*> (rgb_data);
- }
- else
- data = reinterpret_cast<const void*> (image->getMetaData ().Data ());
- image_viewer_.addRGBImage ((const unsigned char*) data, image->getWidth (), image->getHeight ());
-
- if (continuous_ || trigger_)
- {
- importer_->SetWholeExtent (0, image->getWidth () - 1, 0, image->getHeight () - 1, 0, 0);
- importer_->SetDataExtentToWholeExtent ();
-
- std::stringstream ss;
- ss << "frame_" + time + "_rgb.tiff";
- importer_->SetImportVoidPointer (const_cast<void*>(data), 1);
- importer_->Update ();
- flipper_->SetInputConnection (importer_->GetOutputPort ());
- flipper_->Update ();
- writer_->SetFileName (ss.str ().c_str ());
- writer_->SetInputConnection (flipper_->GetOutputPort ());
- writer_->Write ();
- std::cout << "writing rgb frame: " << ss.str () << std::endl;
- }
- } // if (image_)
-
-
- if (depth_image)
- {
- if (depth_data)
- delete[] depth_data;
- depth_data = pcl::visualization::FloatImageUtils::getVisualImage (
- reinterpret_cast<const unsigned short*> (depth_image->getDepthMetaData ().Data ()),
- depth_image->getWidth (), depth_image->getHeight (),
- std::numeric_limits<unsigned short>::min (),
- // Scale so that the colors look brigher on screen
- std::numeric_limits<unsigned short>::max () / 10,
- true);
-
- depth_image_viewer_.addRGBImage (depth_data, depth_image->getWidth (), depth_image->getHeight ());
- if (continuous_ || trigger_)
- {
- std::stringstream ss;
- ss << "frame_" + time + "_depth.tiff";
-
- depth_importer_->SetWholeExtent (0, depth_image->getWidth () - 1, 0, depth_image->getHeight () - 1, 0, 0);
- depth_importer_->SetDataExtentToWholeExtent ();
- depth_importer_->SetImportVoidPointer (const_cast<void*> (reinterpret_cast<const void*> (depth_image->getDepthMetaData ().Data ())), 1);
- depth_importer_->Update ();
- flipper_->SetInputConnection (depth_importer_->GetOutputPort ());
- flipper_->Update ();
- writer_->SetFileName (ss.str ().c_str ());
- writer_->SetInputConnection (flipper_->GetOutputPort ());
- writer_->Write ();
- std::cout << "writing depth frame: " << ss.str () << std::endl;
- }
+ }
+
+ if (depth_image) {
+ delete[] depth_data;
+ depth_data = pcl::visualization::FloatImageUtils::getVisualImage(
+ reinterpret_cast<const unsigned short*>(
+ depth_image->getDepthMetaData().Data()),
+ depth_image->getWidth(),
+ depth_image->getHeight(),
+ std::numeric_limits<unsigned short>::min(),
+ // Scale so that the colors look brigher on screen
+ std::numeric_limits<unsigned short>::max() / 10,
+ true);
+
+ depth_image_viewer_.addRGBImage(
+ depth_data, depth_image->getWidth(), depth_image->getHeight());
+ if (continuous_ || trigger_) {
+ std::stringstream ss;
+ ss << "frame_" + time + "_depth.tiff";
+
+ depth_importer_->SetWholeExtent(
+ 0, depth_image->getWidth() - 1, 0, depth_image->getHeight() - 1, 0, 0);
+ depth_importer_->SetDataExtentToWholeExtent();
+ depth_importer_->SetImportVoidPointer(
+ const_cast<void*>(reinterpret_cast<const void*>(
+ depth_image->getDepthMetaData().Data())),
+ 1);
+ depth_importer_->Update();
+ flipper_->SetInputConnection(depth_importer_->GetOutputPort());
+ flipper_->Update();
+ writer_->SetFileName(ss.str().c_str());
+ writer_->SetInputConnection(flipper_->GetOutputPort());
+ writer_->Write();
+ std::cout << "writing depth frame: " << ss.str() << std::endl;
}
- trigger_ = false;
- image_viewer_.spinOnce ();
- depth_image_viewer_.spinOnce ();
}
-
- image_mutex_.lock ();
- // stop the grabber
- grabber_.stop ();
- image_mutex_.unlock ();
- return 0;
+ trigger_ = false;
+ image_viewer_.spinOnce();
+ depth_image_viewer_.spinOnce();
}
- pcl::OpenNIGrabber& grabber_;
- pcl::visualization::ImageViewer image_viewer_;
- pcl::visualization::ImageViewer depth_image_viewer_;
- bool quit_;
- bool continuous_;
- bool trigger_;
- mutable std::mutex image_mutex_;
- openni_wrapper::Image::Ptr image_;
- openni_wrapper::DepthImage::Ptr depth_image_;
- vtkSmartPointer<vtkImageImport> importer_, depth_importer_;
- vtkSmartPointer<vtkTIFFWriter> writer_;
- vtkSmartPointer<vtkImageFlip> flipper_;
+ image_mutex_.lock();
+ // stop the grabber
+ grabber_.stop();
+ image_mutex_.unlock();
+ return 0;
+ }
+
+ pcl::OpenNIGrabber& grabber_;
+ pcl::visualization::ImageViewer image_viewer_;
+ pcl::visualization::ImageViewer depth_image_viewer_;
+ bool quit_;
+ bool continuous_;
+ bool trigger_;
+ mutable std::mutex image_mutex_;
+ openni_wrapper::Image::Ptr image_;
+ openni_wrapper::DepthImage::Ptr depth_image_;
+ vtkSmartPointer<vtkImageImport> importer_, depth_importer_;
+ vtkSmartPointer<vtkTIFFWriter> writer_;
+ vtkSmartPointer<vtkImageFlip> flipper_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
- std::cout << "usage: " << argv[0] << " [((<device_id> | <path-to-oni-file>) [-imagemode <mode>] [-depthformat <format>] [-paused] | -l [<device_id>]| -h | --help)]" << std::endl;
+ std::cout << "usage: " << argv[0]
+ << " [((<device_id> | <path-to-oni-file>) [-imagemode <mode>] "
+ "[-depthformat <format>] [-paused] | -l [<device_id>]| -h | --help)]"
+ << std::endl;
std::cout << argv[0] << " -h | --help : shows this help" << std::endl;
std::cout << argv[0] << " -l : list all available devices" << std::endl;
- std::cout << argv[0] << " -l <device-id> : list all available modes for specified device" << std::endl;
+ std::cout << argv[0]
+ << " -l <device-id> : list all available modes for specified device"
+ << std::endl;
- std::cout << " device_id may be #1, #2, ... for the first, second etc device in the list"
+ std::cout << " device_id may be #1, #2, ... for the first, second "
+ "etc device in the list"
#ifndef _WIN32
- << " or" << std::endl
- << " bus@address for the device connected to a specific usb-bus / address combination or" << std::endl
- << " <serial-number>"
+ << " or" << std::endl
+ << " bus@address for the device connected to a specific "
+ "usb-bus / address combination or"
+ << std::endl
+ << " <serial-number>"
#endif
- << std::endl;
- std::cout << " -paused : start grabber in paused mode. Toggle pause by pressing the space bar\n";
- std::cout << " or grab single frames by just pressing the left mouse button.\n";
+ << std::endl;
+ std::cout << " -paused : start grabber in paused mode. Toggle pause "
+ "by pressing the space bar\n";
+ std::cout << " or grab single frames by just pressing the "
+ "left mouse button.\n";
std::cout << std::endl;
std::cout << "examples:" << std::endl;
std::cout << argv[0] << " \"#1\"" << std::endl;
std::cout << " uses the first device." << std::endl;
std::cout << argv[0] << " \"./temp/test.oni\"" << std::endl;
- std::cout << " uses the oni-player device to play back oni file given by path." << std::endl;
+ std::cout << " uses the oni-player device to play back oni file given by path."
+ << std::endl;
std::cout << argv[0] << " -l" << std::endl;
std::cout << " lists all available devices." << std::endl;
std::cout << argv[0] << " -l \"#2\"" << std::endl;
std::cout << " lists all available modes for the second device" << std::endl;
#ifndef _WIN32
std::cout << argv[0] << " A00361800903049A" << std::endl;
- std::cout << " uses the device with the serial number \'A00361800903049A\'." << std::endl;
+ std::cout << " uses the device with the serial number \'A00361800903049A\'."
+ << std::endl;
std::cout << argv[0] << " 1@16" << std::endl;
std::cout << " uses the device on address 16 at USB bus 1." << std::endl;
-#endif
+#endif
}
-int
-main (int argc, char** argv)
+int
+main(int argc, char** argv)
{
- std::string device_id ("");
+ std::string device_id("");
pcl::OpenNIGrabber::Mode image_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode;
-
- if (argc >= 2)
- {
+
+ if (argc >= 2) {
device_id = argv[1];
- if (device_id == "--help" || device_id == "-h")
- {
- usage (argv);
- return (0);
+ if (device_id == "--help" || device_id == "-h") {
+ usage(argv);
+ return 0;
}
- else if (device_id == "-l")
- {
- if (argc >= 3)
- {
- pcl::OpenNIGrabber grabber (argv[2]);
- auto device = grabber.getDevice ();
- std::vector<std::pair<int, XnMapOutputMode> > modes;
-
- if (device->hasImageStream ())
- {
- std::cout << std::endl << "Supported image modes for device: " << device->getVendorName () << " , " << device->getProductName () << std::endl;
- modes = grabber.getAvailableImageModes ();
- for (std::vector<std::pair<int, XnMapOutputMode> >::const_iterator it = modes.begin (); it != modes.end (); ++it)
- {
- std::cout << it->first << " = " << it->second.nXRes << " x " << it->second.nYRes << " @ " << it->second.nFPS << std::endl;
+ else if (device_id == "-l") {
+ if (argc >= 3) {
+ pcl::OpenNIGrabber grabber(argv[2]);
+ auto device = grabber.getDevice();
+ std::vector<std::pair<int, XnMapOutputMode>> modes;
+
+ if (device->hasImageStream()) {
+ std::cout << std::endl
+ << "Supported image modes for device: " << device->getVendorName()
+ << " , " << device->getProductName() << std::endl;
+ modes = grabber.getAvailableImageModes();
+ for (std::vector<std::pair<int, XnMapOutputMode>>::const_iterator it =
+ modes.begin();
+ it != modes.end();
+ ++it) {
+ std::cout << it->first << " = " << it->second.nXRes << " x "
+ << it->second.nYRes << " @ " << it->second.nFPS << std::endl;
}
}
}
- else
- {
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
- std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
- << ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
+ else {
+ openni_wrapper::OpenNIDriver& driver =
+ openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices();
+ ++deviceIdx) {
+ std::cout << "Device: " << deviceIdx + 1
+ << ", vendor: " << driver.getVendorName(deviceIdx)
+ << ", product: " << driver.getProductName(deviceIdx)
+ << ", connected: " << driver.getBus(deviceIdx) << " @ "
+ << driver.getAddress(deviceIdx) << ", serial number: \'"
+ << driver.getSerialNumber(deviceIdx) << "\'" << std::endl;
}
-
}
else
std::cout << "No devices connected." << std::endl;
-
- std::cout <<"Virtual Devices available: ONI player" << std::endl;
+
+ std::cout << "Virtual Devices available: ONI player" << std::endl;
}
- return (0);
+ return 0;
}
}
- else
- {
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
+ else {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
if (driver.getNumberDevices() > 0)
std::cout << "Device Id not set, using first device." << std::endl;
}
-
+
unsigned mode;
- if (pcl::console::parse (argc, argv, "-imagemode", mode) != -1)
- image_mode = static_cast<pcl::OpenNIGrabber::Mode> (mode);
-
+ if (pcl::console::parse(argc, argv, "-imagemode", mode) != -1)
+ image_mode = static_cast<pcl::OpenNIGrabber::Mode>(mode);
+
int depthformat = openni_wrapper::OpenNIDevice::OpenNI_12_bit_depth;
- pcl::console::parse_argument (argc, argv, "-depthformat", depthformat);
+ pcl::console::parse_argument(argc, argv, "-depthformat", depthformat);
- pcl::OpenNIGrabber grabber (device_id, pcl::OpenNIGrabber::OpenNI_Default_Mode, image_mode);
+ pcl::OpenNIGrabber grabber(
+ device_id, pcl::OpenNIGrabber::OpenNI_Default_Mode, image_mode);
// Set the depth output format
- grabber.getDevice ()->setDepthOutputFormat (static_cast<openni_wrapper::OpenNIDevice::DepthMode> (depthformat));
+ grabber.getDevice()->setDepthOutputFormat(
+ static_cast<openni_wrapper::OpenNIDevice::DepthMode>(depthformat));
- bool paused = find_switch (argc, argv, "-paused");
-
- OpenNIGrabFrame grabFrame (grabber, paused);
- return grabFrame.run ();
-}
+ bool paused = find_switch(argc, argv, "-paused");
+ OpenNIGrabFrame grabFrame(grabber, paused);
+ return grabFrame.run();
+}
*
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/features/integral_image_normal.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
#include <thread>
using namespace std::chrono_literals;
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
template <typename PointType>
-class OpenNIIntegralImageNormalEstimation
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNIIntegralImageNormalEstimation (const std::string& device_id = "")
- : viewer ("PCL OpenNI NormalEstimation Viewer")
- , device_id_(device_id)
- {
- //ne_.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::AVERAGE_3D_GRADIENT);
- ne_.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::COVARIANCE_MATRIX);
- ne_.setNormalSmoothingSize (11.0);
- new_cloud_ = false;
- viewer.registerKeyboardCallback(&OpenNIIntegralImageNormalEstimation::keyboard_callback, *this);
- }
+class OpenNIIntegralImageNormalEstimation {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNIIntegralImageNormalEstimation(const std::string& device_id = "")
+ : viewer("PCL OpenNI NormalEstimation Viewer"), device_id_(device_id)
+ {
+ ne_.setNormalEstimationMethod(
+ pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::COVARIANCE_MATRIX);
+ ne_.setNormalSmoothingSize(11.0);
+ new_cloud_ = false;
+ viewer.registerKeyboardCallback(
+ &OpenNIIntegralImageNormalEstimation::keyboard_callback, *this);
+ }
+ void
+ cloud_cb(const CloudConstPtr& cloud)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ // lock while we set our cloud;
- void
- cloud_cb (const CloudConstPtr& cloud)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- //lock while we set our cloud;
- //FPS_CALC ("computation");
- // Estimate surface normals
+ normals_.reset(new pcl::PointCloud<pcl::Normal>);
- normals_.reset (new pcl::PointCloud<pcl::Normal>);
+ ne_.setInputCloud(cloud);
+ ne_.compute(*normals_);
+ cloud_ = cloud;
- //double start = pcl::getTime ();
- ne_.setInputCloud (cloud);
- ne_.compute (*normals_);
- //double stop = pcl::getTime ();
- //std::cout << "Time for normal estimation: " << (stop - start) * 1000.0 << " ms" << std::endl;
- cloud_ = cloud;
+ new_cloud_ = true;
+ }
- new_cloud_ = true;
+ void
+ viz_cb(pcl::visualization::PCLVisualizer& viz)
+ {
+ mtx_.lock();
+ if (!cloud_ || !normals_) {
+ mtx_.unlock();
+ return;
}
- void
- viz_cb (pcl::visualization::PCLVisualizer& viz)
- {
- mtx_.lock ();
- if (!cloud_ || !normals_)
- {
- mtx_.unlock ();
- return;
- }
-
- CloudConstPtr temp_cloud;
- pcl::PointCloud<pcl::Normal>::Ptr temp_normals;
- temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
- temp_normals.swap (normals_);
- mtx_.unlock ();
-
- if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
- {
- viz.addPointCloud (temp_cloud, "OpenNICloud");
- viz.resetCameraViewpoint ("OpenNICloud");
- }
- // Render the data
- if (new_cloud_)
- {
- viz.removePointCloud ("normalcloud");
- viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, temp_normals, 100, 0.05f, "normalcloud");
- new_cloud_ = false;
- }
- }
+ CloudConstPtr temp_cloud;
+ pcl::PointCloud<pcl::Normal>::Ptr temp_normals;
+ temp_cloud.swap(cloud_); // here we set cloud_ to null, so that
+ temp_normals.swap(normals_);
+ mtx_.unlock();
- void
- keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- switch (event.getKeyCode ())
- {
- case '1':
- ne_.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::COVARIANCE_MATRIX);
- std::cout << "switched to COVARIANCE_MATRIX method\n";
- break;
- case '2':
- ne_.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::AVERAGE_3D_GRADIENT);
- std::cout << "switched to AVERAGE_3D_GRADIENT method\n";
- break;
- case '3':
- ne_.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::AVERAGE_DEPTH_CHANGE);
- std::cout << "switched to AVERAGE_DEPTH_CHANGE method\n";
- break;
- case '4':
- ne_.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::SIMPLE_3D_GRADIENT);
- std::cout << "switched to SIMPLE_3D_GRADIENT method\n";
- break;
- }
+ if (!viz.updatePointCloud(temp_cloud, "OpenNICloud")) {
+ viz.addPointCloud(temp_cloud, "OpenNICloud");
+ viz.resetCameraViewpoint("OpenNICloud");
+ }
+ // Render the data
+ if (new_cloud_) {
+ viz.removePointCloud("normalcloud");
+ viz.addPointCloudNormals<PointType, pcl::Normal>(
+ temp_cloud, temp_normals, 100, 0.05f, "normalcloud");
+ new_cloud_ = false;
}
+ }
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
+ void
+ keyboard_callback(const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ switch (event.getKeyCode()) {
+ case '1':
+ ne_.setNormalEstimationMethod(
+ pcl::IntegralImageNormalEstimation<PointType,
+ pcl::Normal>::COVARIANCE_MATRIX);
+ std::cout << "switched to COVARIANCE_MATRIX method\n";
+ break;
+ case '2':
+ ne_.setNormalEstimationMethod(
+ pcl::IntegralImageNormalEstimation<PointType,
+ pcl::Normal>::AVERAGE_3D_GRADIENT);
+ std::cout << "switched to AVERAGE_3D_GRADIENT method\n";
+ break;
+ case '3':
+ ne_.setNormalEstimationMethod(
+ pcl::IntegralImageNormalEstimation<PointType,
+ pcl::Normal>::AVERAGE_DEPTH_CHANGE);
+ std::cout << "switched to AVERAGE_DEPTH_CHANGE method\n";
+ break;
+ case '4':
+ ne_.setNormalEstimationMethod(
+ pcl::IntegralImageNormalEstimation<PointType,
+ pcl::Normal>::SIMPLE_3D_GRADIENT);
+ std::cout << "switched to SIMPLE_3D_GRADIENT method\n";
+ break;
+ }
+ }
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
- viewer.runOnVisualizationThread ([this] (pcl::visualization::PCLVisualizer& viz) { viz_cb (viz); }, "viz_cb");
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- interface.start ();
+ viewer.runOnVisualizationThread(
+ [this](pcl::visualization::PCLVisualizer& viz) { viz_cb(viz); }, "viz_cb");
- while (!viewer.wasStopped ())
- {
- std::this_thread::sleep_for(1s);
- }
+ interface.start();
- interface.stop ();
+ while (!viewer.wasStopped()) {
+ std::this_thread::sleep_for(1s);
}
- pcl::IntegralImageNormalEstimation<PointType, pcl::Normal> ne_;
- pcl::visualization::CloudViewer viewer;
- std::string device_id_;
- std::mutex mtx_;
- // Data
- pcl::PointCloud<pcl::Normal>::Ptr normals_;
- CloudConstPtr cloud_;
- bool new_cloud_;
+ interface.stop();
+ }
+
+ pcl::IntegralImageNormalEstimation<PointType, pcl::Normal> ne_;
+ pcl::visualization::CloudViewer viewer;
+ std::string device_id_;
+ std::mutex mtx_;
+ // Data
+ pcl::PointCloud<pcl::Normal>::Ptr normals_;
+ CloudConstPtr cloud_;
+ bool new_cloud_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " [<device_id>]\n\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
std::string arg;
if (argc > 1)
- arg = std::string (argv[1]);
+ arg = std::string(argv[1]);
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (arg == "--help" || arg == "-h" || driver.getNumberDevices () == 0)
- {
- usage (argv);
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (arg == "--help" || arg == "-h" || driver.getNumberDevices() == 0) {
+ usage(argv);
return 1;
}
+ // clang-format off
std::cout << "Press following keys to switch to the different integral image normal estimation methods:\n";
std::cout << "<1> COVARIANCE_MATRIX method\n";
std::cout << "<2> AVERAGE_3D_GRADIENT method\n";
std::cout << "<3> AVERAGE_DEPTH_CHANGE method\n";
std::cout << "<4> SIMPLE_3D_GRADIENT method\n";
std::cout << "<Q,q> quit\n\n";
+ // clang-format on
- pcl::OpenNIGrabber grabber ("");
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- PCL_INFO ("PointXYZRGBA mode enabled.\n");
- OpenNIIntegralImageNormalEstimation<pcl::PointXYZRGBA> v ("");
- v.run ();
+ pcl::OpenNIGrabber grabber("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ PCL_INFO("PointXYZRGBA mode enabled.\n");
+ OpenNIIntegralImageNormalEstimation<pcl::PointXYZRGBA> v("");
+ v.run();
}
- else
- {
- PCL_INFO ("PointXYZ mode enabled.\n");
- OpenNIIntegralImageNormalEstimation<pcl::PointXYZ> v ("");
- v.run ();
+ else {
+ PCL_INFO("PointXYZ mode enabled.\n");
+ OpenNIIntegralImageNormalEstimation<pcl::PointXYZ> v("");
+ v.run();
}
- return (0);
+ return 0;
}
#define MEASURE_FUNCTION_TIME
#include <pcl/common/time.h> //fps calculations
-#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/boost.h>
-#include <pcl/visualization/image_viewer.h>
-#include <pcl/console/print.h>
#include <pcl/console/parse.h>
-#include <pcl/tracking/pyramidal_klt.h>
+#include <pcl/console/print.h>
+#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/harris_2d.h>
+#include <pcl/tracking/pyramidal_klt.h>
+#include <pcl/visualization/boost.h>
+#include <pcl/visualization/image_viewer.h>
#include <mutex>
#define SHOW_FPS 1
#if SHOW_FPS
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
#else
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
-}while(false)
+#define FPS_CALC(_WHAT_) \
+ do { \
+ } while (false)
#endif
void
-printHelp (int, char **argv)
+printHelp(int, char** argv)
{
using pcl::console::print_error;
using pcl::console::print_info;
+ // clang-format off
print_error ("Syntax is: %s [((<device_id> | <path-to-oni-file>) [-depthmode <mode>] [-imagemode <mode>] [-xyz] | -l [<device_id>]| -h | --help)]\n", argv [0]);
print_info ("%s -h | --help : shows this help\n", argv [0]);
print_info ("%s -xyz : use only XYZ values and ignore RGB components (this flag is required for use with ASUS Xtion Pro) \n", argv [0]);
print_info ("\t\t bus@address for the device connected to a specific usb-bus / address combination\n");
print_info ("\t\t <serial-number>\n");
#endif
- print_info ("\n\nexamples:\n");
- print_info ("%s \"#1\"\n", argv [0]);
- print_info ("\t\t uses the first device.\n");
- print_info ("%s \"./temp/test.oni\"\n", argv [0]);
- print_info ("\t\t uses the oni-player device to play back oni file given by path.\n");
- print_info ("%s -l\n", argv [0]);
- print_info ("\t\t list all available devices.\n");
- print_info ("%s -l \"#2\"\n", argv [0]);
- print_info ("\t\t list all available modes for the second device.\n");
- #ifndef _WIN32
- print_info ("%s A00361800903049A\n", argv [0]);
- print_info ("\t\t uses the device with the serial number \'A00361800903049A\'.\n");
- print_info ("%s 1@16\n", argv [0]);
- print_info ("\t\t uses the device on address 16 at USB bus 1.\n");
- #endif
+ print_info("\n\nexamples:\n");
+ print_info("%s \"#1\"\n", argv[0]);
+ print_info("\t\t uses the first device.\n");
+ print_info("%s \"./temp/test.oni\"\n", argv[0]);
+ print_info("\t\t uses the oni-player device to play back oni file given by path.\n");
+ print_info("%s -l\n", argv[0]);
+ print_info("\t\t list all available devices.\n");
+ print_info("%s -l \"#2\"\n", argv[0]);
+ print_info("\t\t list all available modes for the second device.\n");
+#ifndef _WIN32
+ print_info("%s A00361800903049A\n", argv[0]);
+ print_info("\t\t uses the device with the serial number \'A00361800903049A\'.\n");
+ print_info("%s 1@16\n", argv[0]);
+ print_info("\t\t uses the device on address 16 at USB bus 1.\n");
+#endif
+ // clang-format on
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointType>
-class OpenNIViewer
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNIViewer (pcl::Grabber& grabber)
- : grabber_ (grabber)
- , rgb_data_ (nullptr), rgb_data_size_ (0)
- { }
-
- void
- detect_keypoints (const CloudConstPtr& cloud)
- {
- pcl::HarrisKeypoint2D<PointType, pcl::PointXYZI> harris;
- harris.setInputCloud (cloud);
- harris.setNumberOfThreads (6);
- harris.setNonMaxSupression (true);
- harris.setRadiusSearch (0.01);
- harris.setMethod (pcl::HarrisKeypoint2D<PointType, pcl::PointXYZI>::TOMASI);
- harris.setThreshold (0.05);
- harris.setWindowWidth (5);
- harris.setWindowHeight (5);
- pcl::PointCloud<pcl::PointXYZI>::Ptr response (new pcl::PointCloud<pcl::PointXYZI>);
- harris.compute (*response);
- points_ = harris.getKeypointsIndices ();
- }
+class OpenNIViewer {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudConstPtr = typename Cloud::ConstPtr;
- void
- cloud_callback (const CloudConstPtr& cloud)
- {
- FPS_CALC ("cloud callback");
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- cloud_ = cloud;
- // Compute Tomasi keypoints
- tracker_->setInputCloud (cloud_);
- // if (!points_)
- // {
- if (!points_ || (counter_ % 10 == 0))
- {
- detect_keypoints (cloud_);
- tracker_->setPointsToTrack (points_);
- }
+ OpenNIViewer(pcl::Grabber& grabber)
+ : grabber_(grabber), rgb_data_(nullptr), rgb_data_size_(0)
+ {}
- // }
- tracker_->compute ();
- ++counter_;
- }
+ void
+ detect_keypoints(const CloudConstPtr& cloud)
+ {
+ pcl::HarrisKeypoint2D<PointType, pcl::PointXYZI> harris;
+ harris.setInputCloud(cloud);
+ harris.setNumberOfThreads(6);
+ harris.setNonMaxSupression(true);
+ harris.setRadiusSearch(0.01);
+ harris.setMethod(pcl::HarrisKeypoint2D<PointType, pcl::PointXYZI>::TOMASI);
+ harris.setThreshold(0.05);
+ harris.setWindowWidth(5);
+ harris.setWindowHeight(5);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr response(new pcl::PointCloud<pcl::PointXYZI>);
+ harris.compute(*response);
+ points_ = harris.getKeypointsIndices();
+ }
- void
- image_callback (const openni_wrapper::Image::Ptr& image)
- {
- FPS_CALC ("image callback");
- std::lock_guard<std::mutex> lock (image_mutex_);
- image_ = image;
-
- if (image->getEncoding () != openni_wrapper::Image::RGB)
- {
- if (rgb_data_size_ < image->getWidth () * image->getHeight ())
- {
- if (rgb_data_)
- delete [] rgb_data_;
- rgb_data_size_ = image->getWidth () * image->getHeight ();
- rgb_data_ = new unsigned char [rgb_data_size_ * 3];
- }
- image_->fillRGB (image_->getWidth (), image_->getHeight (), rgb_data_);
- }
+ void
+ cloud_callback(const CloudConstPtr& cloud)
+ {
+ FPS_CALC("cloud callback");
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ cloud_ = cloud;
+ // Compute Tomasi keypoints
+ tracker_->setInputCloud(cloud_);
+ if (!points_ || (counter_ % 10 == 0)) {
+ detect_keypoints(cloud_);
+ tracker_->setPointsToTrack(points_);
}
+ tracker_->compute();
+ ++counter_;
+ }
- void
- keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
- {
- static pcl::PCDWriter writer;
- static std::ostringstream frame;
- if (event.keyUp ())
- {
- if ((event.getKeyCode () == 's') || (event.getKeyCode () == 'S'))
- {
- std::lock_guard<std::mutex> lock (cloud_mutex_);
- frame.str ("frame-");
- frame << boost::posix_time::to_iso_string (boost::posix_time::microsec_clock::local_time ()) << ".pcd";
- writer.writeBinaryCompressed (frame.str (), *cloud_);
- PCL_INFO ("Written cloud %s.\n", frame.str ().c_str ());
- }
+ void
+ image_callback(const openni_wrapper::Image::Ptr& image)
+ {
+ FPS_CALC("image callback");
+ std::lock_guard<std::mutex> lock(image_mutex_);
+ image_ = image;
+
+ if (image->getEncoding() != openni_wrapper::Image::RGB) {
+ if (rgb_data_size_ < image->getWidth() * image->getHeight()) {
+ delete[] rgb_data_;
+ rgb_data_size_ = image->getWidth() * image->getHeight();
+ rgb_data_ = new unsigned char[rgb_data_size_ * 3];
}
+ image_->fillRGB(image_->getWidth(), image_->getHeight(), rgb_data_);
}
+ }
- void
- mouse_callback (const pcl::visualization::MouseEvent& mouse_event, void*)
- {
- if (mouse_event.getType() == pcl::visualization::MouseEvent::MouseButtonPress && mouse_event.getButton() == pcl::visualization::MouseEvent::LeftButton)
- {
- std::cout << "left button pressed @ " << mouse_event.getX () << " , " << mouse_event.getY () << std::endl;
+ void
+ keyboard_callback(const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ static pcl::PCDWriter writer;
+ static std::ostringstream frame;
+ if (event.keyUp()) {
+ if ((event.getKeyCode() == 's') || (event.getKeyCode() == 'S')) {
+ std::lock_guard<std::mutex> lock(cloud_mutex_);
+ frame.str("frame-");
+ frame << boost::posix_time::to_iso_string(
+ boost::posix_time::microsec_clock::local_time())
+ << ".pcd";
+ writer.writeBinaryCompressed(frame.str(), *cloud_);
+ PCL_INFO("Written cloud %s.\n", frame.str().c_str());
}
}
+ }
- /**
- * @brief starts the main loop
- */
- void
- run ()
- {
- std::function<void (const CloudConstPtr&) > cloud_cb = [this] (const CloudConstPtr& cloud) { cloud_callback (cloud); };
- boost::signals2::connection cloud_connection = grabber_.registerCallback (cloud_cb);
-
- boost::signals2::connection image_connection;
- if (grabber_.providesCallback<void (const openni_wrapper::Image::Ptr&)>())
- {
- image_viewer_.reset (new pcl::visualization::ImageViewer ("Pyramidal KLT Tracker"));
- std::function<void (const openni_wrapper::Image::Ptr&) > image_cb = [this] (const openni_wrapper::Image::Ptr& img) { image_callback (img); };
- image_connection = grabber_.registerCallback (image_cb);
- }
+ void
+ mouse_callback(const pcl::visualization::MouseEvent& mouse_event, void*)
+ {
+ if (mouse_event.getType() == pcl::visualization::MouseEvent::MouseButtonPress &&
+ mouse_event.getButton() == pcl::visualization::MouseEvent::LeftButton) {
+ std::cout << "left button pressed @ " << mouse_event.getX() << " , "
+ << mouse_event.getY() << std::endl;
+ }
+ }
- tracker_.reset (new pcl::tracking::PyramidalKLTTracker<PointType>);
+ /**
+ * \brief starts the main loop
+ */
+ void
+ run()
+ {
+ std::function<void(const CloudConstPtr&)> cloud_cb =
+ [this](const CloudConstPtr& cloud) { cloud_callback(cloud); };
+ boost::signals2::connection cloud_connection = grabber_.registerCallback(cloud_cb);
+
+ boost::signals2::connection image_connection;
+ if (grabber_.providesCallback<void(const openni_wrapper::Image::Ptr&)>()) {
+ image_viewer_.reset(new pcl::visualization::ImageViewer("Pyramidal KLT Tracker"));
+ std::function<void(const openni_wrapper::Image::Ptr&)> image_cb =
+ [this](const openni_wrapper::Image::Ptr& img) { image_callback(img); };
+ image_connection = grabber_.registerCallback(image_cb);
+ }
- bool image_init = false;
+ tracker_.reset(new pcl::tracking::PyramidalKLTTracker<PointType>);
- grabber_.start ();
+ bool image_init = false;
- while (!image_viewer_->wasStopped ())
- {
- openni_wrapper::Image::Ptr image;
- CloudConstPtr cloud;
+ grabber_.start();
- // See if we can get a cloud
- if (cloud_mutex_.try_lock ())
- {
- cloud_.swap (cloud);
- cloud_mutex_.unlock ();
- }
+ while (!image_viewer_->wasStopped()) {
+ openni_wrapper::Image::Ptr image;
+ CloudConstPtr cloud;
- // See if we can get an image
- if (image_mutex_.try_lock ())
- {
- image_.swap (image);
- image_mutex_.unlock ();
- }
+ // See if we can get a cloud
+ if (cloud_mutex_.try_lock()) {
+ cloud_.swap(cloud);
+ cloud_mutex_.unlock();
+ }
- if (image)
- {
- if (!image_init && cloud && cloud->width != 0)
- {
- image_viewer_->setPosition (0, 0);
- image_viewer_->setSize (cloud->width, cloud->height);
- image_init = true;
- }
+ // See if we can get an image
+ if (image_mutex_.try_lock()) {
+ image_.swap(image);
+ image_mutex_.unlock();
+ }
- if (image->getEncoding() == openni_wrapper::Image::RGB)
- image_viewer_->addRGBImage (image->getMetaData ().Data (), image->getWidth (), image->getHeight ());
- else
- image_viewer_->addRGBImage (rgb_data_, image->getWidth (), image->getHeight ());
- image_viewer_->spinOnce ();
+ if (image) {
+ if (!image_init && cloud && cloud->width != 0) {
+ image_viewer_->setPosition(0, 0);
+ image_viewer_->setSize(cloud->width, cloud->height);
+ image_init = true;
}
- if (tracker_->getInitialized () && cloud_)
- {
- if (points_mutex_.try_lock ())
- {
- keypoints_ = tracker_->getTrackedPoints ();
- points_status_ = tracker_->getPointsToTrackStatus ();
- points_mutex_.unlock ();
- }
+ if (image->getEncoding() == openni_wrapper::Image::RGB)
+ image_viewer_->addRGBImage(
+ image->getMetaData().Data(), image->getWidth(), image->getHeight());
+ else
+ image_viewer_->addRGBImage(rgb_data_, image->getWidth(), image->getHeight());
+ image_viewer_->spinOnce();
+ }
- std::vector<float> markers;
- markers.reserve (keypoints_->size () * 2);
- for (std::size_t i = 0; i < keypoints_->size (); ++i)
- {
- if (points_status_->indices[i] < 0)
- continue;
- const pcl::PointUV &uv = keypoints_->points[i];
- markers.push_back (uv.u);
- markers.push_back (uv.v);
- }
- image_viewer_->removeLayer ("tracked");
- image_viewer_->markPoints (markers, pcl::visualization::blue_color,
- pcl::visualization::red_color, 5, "tracked", 1.0);
+ if (tracker_->getInitialized() && cloud_) {
+ if (points_mutex_.try_lock()) {
+ keypoints_ = tracker_->getTrackedPoints();
+ points_status_ = tracker_->getPointsToTrackStatus();
+ points_mutex_.unlock();
+ }
+ std::vector<float> markers;
+ markers.reserve(keypoints_->size() * 2);
+ for (std::size_t i = 0; i < keypoints_->size(); ++i) {
+ if (points_status_->indices[i] < 0)
+ continue;
+ const pcl::PointUV& uv = keypoints_->points[i];
+ markers.push_back(uv.u);
+ markers.push_back(uv.v);
}
+ image_viewer_->removeLayer("tracked");
+ image_viewer_->markPoints(markers,
+ pcl::visualization::blue_color,
+ pcl::visualization::red_color,
+ 5,
+ "tracked",
+ 1.0);
}
+ }
- grabber_.stop ();
+ grabber_.stop();
- cloud_connection.disconnect ();
- image_connection.disconnect ();
- if (rgb_data_)
- delete[] rgb_data_;
- }
+ cloud_connection.disconnect();
+ image_connection.disconnect();
+ delete[] rgb_data_;
+ }
- pcl::visualization::ImageViewer::Ptr image_viewer_;
-
- pcl::Grabber& grabber_;
- std::mutex cloud_mutex_;
- std::mutex image_mutex_;
- std::mutex points_mutex_;
-
- CloudConstPtr cloud_;
- openni_wrapper::Image::Ptr image_;
- unsigned char* rgb_data_;
- unsigned rgb_data_size_;
-
typename pcl::tracking::PyramidalKLTTracker<PointType>::Ptr tracker_;
-
pcl::PointCloud<pcl::PointUV>::ConstPtr keypoints_;
- pcl::PointIndicesConstPtr points_;
- pcl::PointIndicesConstPtr points_status_;
- int counter_;
+ pcl::visualization::ImageViewer::Ptr image_viewer_;
+
+ pcl::Grabber& grabber_;
+ std::mutex cloud_mutex_;
+ std::mutex image_mutex_;
+ std::mutex points_mutex_;
+
+ CloudConstPtr cloud_;
+ openni_wrapper::Image::Ptr image_;
+ unsigned char* rgb_data_;
+ unsigned rgb_data_size_;
+ typename pcl::tracking::PyramidalKLTTracker<PointType>::Ptr tracker_;
+ pcl::PointCloud<pcl::PointUV>::ConstPtr keypoints_;
+ pcl::PointIndicesConstPtr points_;
+ pcl::PointIndicesConstPtr points_status_;
+ int counter_;
};
// Create the PCLVisualizer object
/* ---[ */
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
std::string device_id;
pcl::OpenNIGrabber::Mode depth_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode;
pcl::OpenNIGrabber::Mode image_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode;
bool xyz = false;
- if (argc >= 2)
- {
+ if (argc >= 2) {
device_id = argv[1];
- if (device_id == "--help" || device_id == "-h")
- {
+ if (device_id == "--help" || device_id == "-h") {
printHelp(argc, argv);
return 0;
}
- if (device_id == "-l")
- {
- if (argc >= 3)
- {
+ if (device_id == "-l") {
+ if (argc >= 3) {
pcl::OpenNIGrabber grabber(argv[2]);
auto device = grabber.getDevice();
- std::cout << "Supported depth modes for device: " << device->getVendorName() << " , " << device->getProductName() << std::endl;
- std::vector<std::pair<int, XnMapOutputMode > > modes = grabber.getAvailableDepthModes();
- for (std::vector<std::pair<int, XnMapOutputMode > >::const_iterator it = modes.begin(); it != modes.end(); ++it)
- {
- std::cout << it->first << " = " << it->second.nXRes << " x " << it->second.nYRes << " @ " << it->second.nFPS << std::endl;
+ std::cout << "Supported depth modes for device: " << device->getVendorName()
+ << " , " << device->getProductName() << std::endl;
+ std::vector<std::pair<int, XnMapOutputMode>> modes =
+ grabber.getAvailableDepthModes();
+ for (std::vector<std::pair<int, XnMapOutputMode>>::const_iterator it =
+ modes.begin();
+ it != modes.end();
+ ++it) {
+ std::cout << it->first << " = " << it->second.nXRes << " x "
+ << it->second.nYRes << " @ " << it->second.nFPS << std::endl;
}
- if (device->hasImageStream ())
- {
- std::cout << std::endl << "Supported image modes for device: " << device->getVendorName() << " , " << device->getProductName() << std::endl;
+ if (device->hasImageStream()) {
+ std::cout << std::endl
+ << "Supported image modes for device: " << device->getVendorName()
+ << " , " << device->getProductName() << std::endl;
modes = grabber.getAvailableImageModes();
- for (std::vector<std::pair<int, XnMapOutputMode > >::const_iterator it = modes.begin(); it != modes.end(); ++it)
- {
- std::cout << it->first << " = " << it->second.nXRes << " x " << it->second.nYRes << " @ " << it->second.nFPS << std::endl;
+ for (std::vector<std::pair<int, XnMapOutputMode>>::const_iterator it =
+ modes.begin();
+ it != modes.end();
+ ++it) {
+ std::cout << it->first << " = " << it->second.nXRes << " x "
+ << it->second.nYRes << " @ " << it->second.nFPS << std::endl;
}
}
}
- else
- {
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
- if (driver.getNumberDevices() > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx)
- {
- std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName(deviceIdx) << ", product: " << driver.getProductName(deviceIdx)
- << ", connected: " << driver.getBus(deviceIdx) << " @ " << driver.getAddress(deviceIdx) << ", serial number: \'" << driver.getSerialNumber(deviceIdx) << "\'" << std::endl;
+ else {
+ openni_wrapper::OpenNIDriver& driver =
+ openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices();
+ ++deviceIdx) {
+ std::cout << "Device: " << deviceIdx + 1
+ << ", vendor: " << driver.getVendorName(deviceIdx)
+ << ", product: " << driver.getProductName(deviceIdx)
+ << ", connected: " << driver.getBus(deviceIdx) << " @ "
+ << driver.getAddress(deviceIdx) << ", serial number: \'"
+ << driver.getSerialNumber(deviceIdx) << "\'" << std::endl;
}
-
}
else
std::cout << "No devices connected." << std::endl;
- std::cout <<"Virtual Devices available: ONI player" << std::endl;
+ std::cout << "Virtual Devices available: ONI player" << std::endl;
}
return 0;
}
}
- else
- {
+ else {
openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
if (driver.getNumberDevices() > 0)
std::cout << "Device Id not set, using first device." << std::endl;
unsigned mode;
if (pcl::console::parse(argc, argv, "-depthmode", mode) != -1)
- depth_mode = pcl::OpenNIGrabber::Mode (mode);
+ depth_mode = pcl::OpenNIGrabber::Mode(mode);
if (pcl::console::parse(argc, argv, "-imagemode", mode) != -1)
- image_mode = pcl::OpenNIGrabber::Mode (mode);
+ image_mode = pcl::OpenNIGrabber::Mode(mode);
- if (pcl::console::find_argument (argc, argv, "-xyz") != -1)
+ if (pcl::console::find_argument(argc, argv, "-xyz") != -1)
xyz = true;
- pcl::OpenNIGrabber grabber (device_id, depth_mode, image_mode);
+ pcl::OpenNIGrabber grabber(device_id, depth_mode, image_mode);
- if (xyz || !grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb> ())
- {
- OpenNIViewer<pcl::PointXYZ> openni_viewer (grabber);
- openni_viewer.run ();
+ if (xyz ||
+ !grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb>()) {
+ OpenNIViewer<pcl::PointXYZ> openni_viewer(grabber);
+ openni_viewer.run();
}
- else
- {
- OpenNIViewer<pcl::PointXYZRGBA> openni_viewer (grabber);
- openni_viewer.run ();
+ else {
+ OpenNIViewer<pcl::PointXYZRGBA> openni_viewer(grabber);
+ openni_viewer.run();
}
- return (0);
+ return 0;
}
/* ]--- */
*
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/console/parse.h>
#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/io/openni_camera/openni_driver.h>
+#include <pcl/io/openni_grabber.h>
#include <pcl/surface/mls.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- if (*stop_computing_) std::cout << "Press 's' to start computing!\n"; \
- } \
-}while(false)
-
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ if (*stop_computing_) \
+ std::cout << "Press 's' to start computing!\n"; \
+ } \
+ } while (false)
+// clang-format on
int default_polynomial_order = 0;
-double default_search_radius = 0.0,
- default_sqr_gauss_param = 0.0;
+double default_search_radius = 0.0, default_sqr_gauss_param = 0.0;
template <typename PointType>
class OpenNISmoothing;
-
-void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event,
- void *stop_void)
+void
+keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* stop_void)
{
- std::shared_ptr<bool> stop = *static_cast<std::shared_ptr<bool>*> (stop_void);
- if (event.getKeySym () == "s" && event.keyDown ())
- {
- *stop = ! *stop;
- if (*stop) std::cout << "Computing is now stopped!\n";
- else std::cout << "Computing commencing!\n";
+ std::shared_ptr<bool> stop = *static_cast<std::shared_ptr<bool>*>(stop_void);
+ if (event.getKeySym() == "s" && event.keyDown()) {
+ *stop = !*stop;
+ if (*stop)
+ std::cout << "Computing is now stopped!\n";
+ else
+ std::cout << "Computing commencing!\n";
}
}
template <typename PointType>
-class OpenNISmoothing
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNISmoothing (double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param,
- int polynomial_order, const std::string& device_id = "")
- : viewer ("PCL OpenNI MLS Smoothing")
- , device_id_(device_id)
- {
- // Start 4 threads
- smoother_.setSearchRadius (search_radius);
- if (sqr_gauss_param_set) smoother_.setSqrGaussParam (sqr_gauss_param);
- smoother_.setPolynomialOrder (polynomial_order);
-
- typename pcl::search::KdTree<PointType>::Ptr tree (new typename pcl::search::KdTree<PointType> ());
- smoother_.setSearchMethod (tree);
-
- viewer.createViewPort (0.0, 0.0, 0.5, 1.0, viewport_input_);
- viewer.setBackgroundColor (0, 0, 0, viewport_input_);
- viewer.createViewPort (0.5, 0.0, 1.0, 1.0, viewport_smoothed_);
- viewer.setBackgroundColor (0, 0, 0, viewport_smoothed_);
-
- stop_computing_.reset (new bool(true));
- cloud_.reset ();
- cloud_smoothed_.reset (new Cloud);
- }
+class OpenNISmoothing {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNISmoothing(double search_radius,
+ bool sqr_gauss_param_set,
+ double sqr_gauss_param,
+ int polynomial_order,
+ const std::string& device_id = "")
+ : viewer("PCL OpenNI MLS Smoothing"), device_id_(device_id)
+ {
+ // Start 4 threads
+ smoother_.setSearchRadius(search_radius);
+ if (sqr_gauss_param_set)
+ smoother_.setSqrGaussParam(sqr_gauss_param);
+ smoother_.setPolynomialOrder(polynomial_order);
+
+ typename pcl::search::KdTree<PointType>::Ptr tree(
+ new typename pcl::search::KdTree<PointType>());
+ smoother_.setSearchMethod(tree);
+
+ viewer.createViewPort(0.0, 0.0, 0.5, 1.0, viewport_input_);
+ viewer.setBackgroundColor(0, 0, 0, viewport_input_);
+ viewer.createViewPort(0.5, 0.0, 1.0, 1.0, viewport_smoothed_);
+ viewer.setBackgroundColor(0, 0, 0, viewport_smoothed_);
+
+ stop_computing_.reset(new bool(true));
+ cloud_.reset();
+ cloud_smoothed_.reset(new Cloud);
+ }
- void
- cloud_cb_ (const CloudConstPtr& cloud)
- {
- FPS_CALC ("computation");
+ void
+ cloud_cb_(const CloudConstPtr& cloud)
+ {
+ FPS_CALC("computation");
- mtx_.lock ();
- if (! *stop_computing_)
- {
- smoother_.setInputCloud (cloud);
- smoother_.process (*cloud_smoothed_);
- }
- cloud_ = cloud;
- mtx_.unlock ();
+ mtx_.lock();
+ if (!*stop_computing_) {
+ smoother_.setInputCloud(cloud);
+ smoother_.process(*cloud_smoothed_);
}
+ cloud_ = cloud;
+ mtx_.unlock();
+ }
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
-
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb_ (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
-
- viewer.registerKeyboardCallback (keyboardEventOccurred, reinterpret_cast<void*> (&stop_computing_));
-
+ viewer.registerKeyboardCallback(keyboardEventOccurred,
+ reinterpret_cast<void*>(&stop_computing_));
- interface.start ();
+ interface.start();
- while (!viewer.wasStopped ())
- {
- FPS_CALC ("visualization");
- viewer.spinOnce ();
+ while (!viewer.wasStopped()) {
+ FPS_CALC("visualization");
+ viewer.spinOnce();
- if (cloud_ && mtx_.try_lock ())
- {
- if (!viewer.updatePointCloud (cloud_, "input_cloud"))
- viewer.addPointCloud (cloud_, "input_cloud", viewport_input_);
- if (! *stop_computing_ && !viewer.updatePointCloud (cloud_smoothed_, "smoothed_cloud"))
- viewer.addPointCloud (cloud_smoothed_, "smoothed_cloud", viewport_smoothed_);
- mtx_.unlock ();
- }
+ if (cloud_ && mtx_.try_lock()) {
+ if (!viewer.updatePointCloud(cloud_, "input_cloud"))
+ viewer.addPointCloud(cloud_, "input_cloud", viewport_input_);
+ if (!*stop_computing_ &&
+ !viewer.updatePointCloud(cloud_smoothed_, "smoothed_cloud"))
+ viewer.addPointCloud(cloud_smoothed_, "smoothed_cloud", viewport_smoothed_);
+ mtx_.unlock();
}
-
- interface.stop ();
}
- pcl::MovingLeastSquares<PointType, PointType> smoother_;
- pcl::visualization::PCLVisualizer viewer;
- std::string device_id_;
- std::mutex mtx_;
- CloudConstPtr cloud_;
- CloudPtr cloud_smoothed_;
- int viewport_input_, viewport_smoothed_;
- std::shared_ptr<bool> stop_computing_;
+ interface.stop();
+ }
+
+ pcl::MovingLeastSquares<PointType, PointType> smoother_;
+ pcl::visualization::PCLVisualizer viewer;
+ std::string device_id_;
+ std::mutex mtx_;
+ CloudConstPtr cloud_;
+ CloudPtr cloud_smoothed_;
+ int viewport_input_, viewport_smoothed_;
+ std::shared_ptr<bool> stop_computing_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
+ // clang-format off
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n"
<< "where options are:\n"
<< " -search_radius X = sphere radius to be used for finding the k-nearest neighbors used for fitting (default: " << default_search_radius << ")\n"
<< " -sqr_gauss_param X = parameter used for the distance based weighting of neighbors (recommended = search_radius^2) (default: " << default_sqr_gauss_param << ")\n"
<< " -polynomial_order X = order of the polynomial to be fit (0 means tangent estimation) (default: " << default_polynomial_order << ")\n";
+ // clang-format on
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
- if (argc < 2)
- {
- usage (argv);
+ if (argc < 2) {
+ usage(argv);
return 1;
}
- std::string arg (argv[1]);
+ std::string arg(argv[1]);
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
bool sqr_gauss_param_set = true;
int polynomial_order = default_polynomial_order;
- pcl::console::parse_argument (argc, argv, "-search_radius", search_radius);
- if (pcl::console::parse_argument (argc, argv, "-sqr_gauss_param", sqr_gauss_param) == -1)
+ pcl::console::parse_argument(argc, argv, "-search_radius", search_radius);
+ if (pcl::console::parse_argument(argc, argv, "-sqr_gauss_param", sqr_gauss_param) ==
+ -1)
sqr_gauss_param_set = false;
- pcl::console::parse_argument (argc, argv, "-polynomial_order", polynomial_order);
+ pcl::console::parse_argument(argc, argv, "-polynomial_order", polynomial_order);
- pcl::OpenNIGrabber grabber (arg);
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- OpenNISmoothing<pcl::PointXYZRGBA> v (search_radius, sqr_gauss_param_set, sqr_gauss_param,
- polynomial_order, arg);
- v.run ();
+ pcl::OpenNIGrabber grabber(arg);
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ OpenNISmoothing<pcl::PointXYZRGBA> v(
+ search_radius, sqr_gauss_param_set, sqr_gauss_param, polynomial_order, arg);
+ v.run();
}
- else
- {
- OpenNISmoothing<pcl::PointXYZ> v (search_radius, sqr_gauss_param_set, sqr_gauss_param,
- polynomial_order, arg);
- v.run ();
+ else {
+ OpenNISmoothing<pcl::PointXYZ> v(
+ search_radius, sqr_gauss_param_set, sqr_gauss_param, polynomial_order, arg);
+ v.run();
}
- return (0);
+ return 0;
}
* POSSIBILITY OF SUCH DAMAGE.
*
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <boost/asio.hpp>
#include <mutex>
#include <thread>
-
using boost::asio::ip::tcp;
using namespace std::chrono_literals;
-struct PointCloudBuffers
-{
+struct PointCloudBuffers {
using Ptr = std::shared_ptr<PointCloudBuffers>;
std::vector<short> points;
std::vector<unsigned char> rgb;
};
void
-CopyPointCloudToBuffers (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud, PointCloudBuffers& cloud_buffers)
+CopyPointCloudToBuffers(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud,
+ PointCloudBuffers& cloud_buffers)
{
- const std::size_t nr_points = cloud->points.size ();
+ const std::size_t nr_points = cloud->points.size();
- cloud_buffers.points.resize (nr_points*3);
- cloud_buffers.rgb.resize (nr_points*3);
+ cloud_buffers.points.resize(nr_points * 3);
+ cloud_buffers.rgb.resize(nr_points * 3);
- const pcl::PointXYZ bounds_min (-0.9f, -0.8f, 1.0f);
- const pcl::PointXYZ bounds_max (0.9f, 3.0f, 3.3f);
+ const pcl::PointXYZ bounds_min(-0.9f, -0.8f, 1.0f);
+ const pcl::PointXYZ bounds_max(0.9f, 3.0f, 3.3f);
std::size_t j = 0;
- for (std::size_t i = 0; i < nr_points; ++i)
- {
+ for (std::size_t i = 0; i < nr_points; ++i) {
const pcl::PointXYZRGBA& point = cloud->points[i];
- if (!std::isfinite (point.x) ||
- !std::isfinite (point.y) ||
- !std::isfinite (point.z))
+ if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z))
continue;
- if (point.x < bounds_min.x ||
- point.y < bounds_min.y ||
- point.z < bounds_min.z ||
- point.x > bounds_max.x ||
- point.y > bounds_max.y ||
- point.z > bounds_max.z)
+ if (point.x < bounds_min.x || point.y < bounds_min.y || point.z < bounds_min.z ||
+ point.x > bounds_max.x || point.y > bounds_max.y || point.z > bounds_max.z)
continue;
const int conversion_factor = 500;
- cloud_buffers.points[j*3 + 0] = static_cast<short> (point.x * conversion_factor);
- cloud_buffers.points[j*3 + 1] = static_cast<short> (point.y * conversion_factor);
- cloud_buffers.points[j*3 + 2] = static_cast<short> (point.z * conversion_factor);
+ cloud_buffers.points[j * 3 + 0] = static_cast<short>(point.x * conversion_factor);
+ cloud_buffers.points[j * 3 + 1] = static_cast<short>(point.y * conversion_factor);
+ cloud_buffers.points[j * 3 + 2] = static_cast<short>(point.z * conversion_factor);
- cloud_buffers.rgb[j*3 + 0] = point.r;
- cloud_buffers.rgb[j*3 + 1] = point.g;
- cloud_buffers.rgb[j*3 + 2] = point.b;
+ cloud_buffers.rgb[j * 3 + 0] = point.r;
+ cloud_buffers.rgb[j * 3 + 1] = point.g;
+ cloud_buffers.rgb[j * 3 + 2] = point.b;
j++;
}
- cloud_buffers.points.resize (j * 3);
- cloud_buffers.rgb.resize (j * 3);
+ cloud_buffers.points.resize(j * 3);
+ cloud_buffers.rgb.resize(j * 3);
}
-
template <typename PointType>
-class PCLMobileServer
-{
- public:
-
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- PCLMobileServer (const std::string& device_id = "", int port = 11111,
- float leaf_size_x = 0.01, float leaf_size_y = 0.01, float leaf_size_z = 0.01)
- : port_ (port),
- device_id_ (device_id),
- viewer_ ("PCL OpenNI Mobile Server")
- {
- voxel_grid_filter_.setLeafSize (leaf_size_x, leaf_size_y, leaf_size_z);
- }
-
- void
- handleIncomingCloud (const CloudConstPtr& new_cloud)
- {
- CloudPtr temp_cloud (new Cloud);
- voxel_grid_filter_.setInputCloud (new_cloud);
- voxel_grid_filter_.filter (*temp_cloud);
-
- PointCloudBuffers::Ptr new_buffers = PointCloudBuffers::Ptr (new PointCloudBuffers);
- CopyPointCloudToBuffers (temp_cloud, *new_buffers);
-
- std::lock_guard<std::mutex> lock (mutex_);
- filtered_cloud_ = temp_cloud;
- buffers_ = new_buffers;
- }
+class PCLMobileServer {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ PCLMobileServer(const std::string& device_id = "",
+ int port = 11111,
+ float leaf_size_x = 0.01,
+ float leaf_size_y = 0.01,
+ float leaf_size_z = 0.01)
+ : port_(port), device_id_(device_id), viewer_("PCL OpenNI Mobile Server")
+ {
+ voxel_grid_filter_.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z);
+ }
- PointCloudBuffers::Ptr
- getLatestBuffers ()
- {
- std::lock_guard<std::mutex> lock (mutex_);
- return (buffers_);
- }
+ void
+ handleIncomingCloud(const CloudConstPtr& new_cloud)
+ {
+ CloudPtr temp_cloud(new Cloud);
+ voxel_grid_filter_.setInputCloud(new_cloud);
+ voxel_grid_filter_.filter(*temp_cloud);
- CloudPtr
- getLatestPointCloud ()
- {
- std::lock_guard<std::mutex> lock (mutex_);
- return (filtered_cloud_);
- }
+ PointCloudBuffers::Ptr new_buffers = PointCloudBuffers::Ptr(new PointCloudBuffers);
+ CopyPointCloudToBuffers(temp_cloud, *new_buffers);
- void
- run ()
- {
- pcl::OpenNIGrabber grabber (device_id_);
- std::function<void (const CloudConstPtr&)> handler_function = [this] (const CloudConstPtr& cloud) { handleIncomingCloud (cloud); };
- grabber.registerCallback (handler_function);
- grabber.start ();
+ std::lock_guard<std::mutex> lock(mutex_);
+ filtered_cloud_ = temp_cloud;
+ buffers_ = new_buffers;
+ }
- // wait for first cloud
- while (!getLatestPointCloud ())
- std::this_thread::sleep_for(10ms);
+ PointCloudBuffers::Ptr
+ getLatestBuffers()
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+ return buffers_;
+ }
- viewer_.showCloud (getLatestPointCloud ());
+ CloudPtr
+ getLatestPointCloud()
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+ return filtered_cloud_;
+ }
+ void
+ run()
+ {
+ pcl::OpenNIGrabber grabber(device_id_);
+ std::function<void(const CloudConstPtr&)> handler_function =
+ [this](const CloudConstPtr& cloud) { handleIncomingCloud(cloud); };
+ grabber.registerCallback(handler_function);
+ grabber.start();
- boost::asio::io_service io_service;
- tcp::endpoint endpoint (tcp::v4 (), static_cast<unsigned short> (port_));
- tcp::acceptor acceptor (io_service, endpoint);
- tcp::socket socket (io_service);
+ // wait for first cloud
+ while (!getLatestPointCloud())
+ std::this_thread::sleep_for(10ms);
- std::cout << "Listening on port " << port_ << "..." << std::endl;
- acceptor.accept (socket);
+ viewer_.showCloud(getLatestPointCloud());
- std::cout << "Client connected." << std::endl;
+ boost::asio::io_service io_service;
+ tcp::endpoint endpoint(tcp::v4(), static_cast<unsigned short>(port_));
+ tcp::acceptor acceptor(io_service, endpoint);
+ tcp::socket socket(io_service);
- double start_time = pcl::getTime ();
- int counter = 0;
+ std::cout << "Listening on port " << port_ << "..." << std::endl;
+ acceptor.accept(socket);
- while (!viewer_.wasStopped ())
- {
+ std::cout << "Client connected." << std::endl;
- // wait for client
- unsigned int nr_points = 0;
- boost::asio::read (socket, boost::asio::buffer (&nr_points, sizeof (nr_points)));
+ double start_time = pcl::getTime();
+ int counter = 0;
- PointCloudBuffers::Ptr buffers_to_send = getLatestBuffers ();
+ while (!viewer_.wasStopped()) {
- nr_points = static_cast<unsigned int> (buffers_to_send->points.size()/3);
- boost::asio::write (socket, boost::asio::buffer (&nr_points, sizeof (nr_points)));
+ // wait for client
+ unsigned int nr_points = 0;
+ boost::asio::read(socket, boost::asio::buffer(&nr_points, sizeof(nr_points)));
- if (nr_points)
- {
- boost::asio::write (socket, boost::asio::buffer (&buffers_to_send->points.front(), nr_points * 3 * sizeof (short)));
- boost::asio::write (socket, boost::asio::buffer (&buffers_to_send->rgb.front(), nr_points * 3 * sizeof (unsigned char)));
- }
+ PointCloudBuffers::Ptr buffers_to_send = getLatestBuffers();
- counter++;
+ nr_points = static_cast<unsigned int>(buffers_to_send->points.size() / 3);
+ boost::asio::write(socket, boost::asio::buffer(&nr_points, sizeof(nr_points)));
- double new_time = pcl::getTime ();
- double elapsed_time = new_time - start_time;
- if (elapsed_time > 1.0)
- {
- double frames_per_second = counter / elapsed_time;
- start_time = new_time;
- counter = 0;
- std::cout << "fps: " << frames_per_second << std::endl;
- }
+ if (nr_points) {
+ boost::asio::write(socket,
+ boost::asio::buffer(&buffers_to_send->points.front(),
+ nr_points * 3 * sizeof(short)));
+ boost::asio::write(socket,
+ boost::asio::buffer(&buffers_to_send->rgb.front(),
+ nr_points * 3 * sizeof(unsigned char)));
+ }
- viewer_.showCloud (getLatestPointCloud ());
+ counter++;
+
+ double new_time = pcl::getTime();
+ double elapsed_time = new_time - start_time;
+ if (elapsed_time > 1.0) {
+ double frames_per_second = counter / elapsed_time;
+ start_time = new_time;
+ counter = 0;
+ std::cout << "fps: " << frames_per_second << std::endl;
}
- grabber.stop ();
+ viewer_.showCloud(getLatestPointCloud());
}
- int port_;
- std::string device_id_;
- std::mutex mutex_;
+ grabber.stop();
+ }
- pcl::VoxelGrid<PointType> voxel_grid_filter_;
- pcl::visualization::CloudViewer viewer_;
+ int port_;
+ std::string device_id_;
+ std::mutex mutex_;
- CloudPtr filtered_cloud_;
- PointCloudBuffers::Ptr buffers_;
+ pcl::VoxelGrid<PointType> voxel_grid_filter_;
+ pcl::visualization::CloudViewer viewer_;
+
+ CloudPtr filtered_cloud_;
+ PointCloudBuffers::Ptr buffers_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <options>\n"
<< "where options are:\n"
<< " -leaf x, y, z :: set the voxel grid leaf size (default: 0.01)\n";
}
-
-int
-main (int argc, char ** argv)
+int
+main(int argc, char** argv)
{
- if (pcl::console::find_argument (argc, argv, "-h") != -1)
- {
- usage (argv);
- return (0);
+ if (pcl::console::find_argument(argc, argv, "-h") != -1) {
+ usage(argv);
+ return 0;
}
int port = 11111;
float leaf_x = 0.01f, leaf_y = 0.01f, leaf_z = 0.01f;
std::string device_id;
- pcl::console::parse_argument (argc, argv, "-port", port);
- pcl::console::parse_3x_arguments (argc, argv, "-leaf", leaf_x, leaf_y, leaf_z, false);
+ pcl::console::parse_argument(argc, argv, "-port", port);
+ pcl::console::parse_3x_arguments(argc, argv, "-leaf", leaf_x, leaf_y, leaf_z, false);
- pcl::OpenNIGrabber grabber ("");
- if (!grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
+ pcl::OpenNIGrabber grabber("");
+ if (!grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
std::cout << "OpenNI grabber does not provide the rgba cloud format." << std::endl;
- return (1);
+ return 1;
}
- PCL_INFO ("Using %f, %f, %f as a leaf size for VoxelGrid.\n", leaf_x, leaf_y, leaf_z);
+ PCL_INFO("Using %f, %f, %f as a leaf size for VoxelGrid.\n", leaf_x, leaf_y, leaf_z);
- PCLMobileServer<pcl::PointXYZRGBA> server (device_id, port, leaf_x, leaf_y, leaf_z);
- server.run ();
+ PCLMobileServer<pcl::PointXYZRGBA> server(device_id, port, leaf_x, leaf_y, leaf_z);
+ server.run();
- return (0);
+ return 0;
}
* Author: Julius Kammerl (julius@kammerl.de)
*/
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/console/parse.h>
#include <pcl/common/time.h>
+#include <pcl/compression/octree_pointcloud_compression.h>
+#include <pcl/console/parse.h>
#include <pcl/filters/passthrough.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
-#include <pcl/compression/octree_pointcloud_compression.h>
+#include <boost/asio.hpp>
#include <cstdio>
#include <cstdlib>
#include <thread>
#include <vector>
-#include <boost/asio.hpp>
-
using boost::asio::ip::tcp;
using namespace pcl;
using namespace std;
using namespace std::chrono_literals;
+// clang-format off
char usage[] = "\n"
" PCL octree point cloud compression\n"
"\n"
" example:\n"
" ./pcl_openni_octree_compression -x -p highC -t -f pc_compressed.pcc \n"
"\n";
-
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+// clang-format on
+
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
void
-print_usage (const std::string &msg)
+print_usage(const std::string& msg)
{
std::cerr << msg << std::endl;
std::cout << usage << std::endl;
}
-class SimpleOpenNIViewer
-{
- public:
- SimpleOpenNIViewer (ostream& outputFile_arg, OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_arg) :
- viewer ("Input Point Cloud - PCL Compression Viewer"),
- outputFile_(outputFile_arg), octreeEncoder_(octreeEncoder_arg)
- {
- }
+class SimpleOpenNIViewer {
+public:
+ SimpleOpenNIViewer(ostream& outputFile_arg,
+ OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_arg)
+ : viewer("Input Point Cloud - PCL Compression Viewer")
+ , outputFile_(outputFile_arg)
+ , octreeEncoder_(octreeEncoder_arg)
+ {}
- void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
- {
- if (!viewer.wasStopped ())
- {
-
PointCloud<PointXYZRGBA>::Ptr cloudOut (new PointCloud<PointXYZRGBA>);
+ void
+ cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
+ {
+ if (!viewer.wasStopped()) {
+
PointCloud<PointXYZRGBA>::Ptr cloudOut(new PointCloud<PointXYZRGBA>);
- octreeEncoder_->encodePointCloud ( cloud, outputFile_);
+ octreeEncoder_->encodePointCloud(cloud, outputFile_);
- viewer.showCloud (cloud);
- }
+ viewer.showCloud(cloud);
}
+ }
- void run ()
- {
-
- // create a new grabber for OpenNI devices
- pcl::OpenNIGrabber interface {};
-
- // make callback function from member function
- std::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
- [this] (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) { cloud_cb_ (cloud); };
+ void
+ run()
+ {
- // connect callback function for desired signal. In this case its a point cloud with color values
- boost::signals2::connection c = interface.registerCallback (f);
+ // create a new grabber for OpenNI devices
+ pcl::OpenNIGrabber interface{};
- // start receiving point clouds
- interface.start ();
+ // make callback function from member function
+ std::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
+ [this](const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+ // connect callback function for desired signal. In this case its a point cloud with
+ // color values
+ boost::signals2::connection c = interface.registerCallback(f);
- while (!outputFile_.fail())
- {
- std::this_thread::sleep_for(1s);
- }
+ // start receiving point clouds
+ interface.start();
- interface.stop ();
+ while (!outputFile_.fail()) {
+ std::this_thread::sleep_for(1s);
}
- pcl::visualization::CloudViewer viewer;
- ostream& outputFile_;
- OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_;
+ interface.stop();
+ }
+
+ pcl::visualization::CloudViewer viewer;
+ ostream& outputFile_;
+ OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_;
};
-struct EventHelper
-{
- EventHelper (ostream& outputFile_arg, OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_arg,
- const std::string& field_name = "z", float min_v = 0, float max_v = 3.0) :
- outputFile_ (outputFile_arg), octreeEncoder_ (octreeEncoder_arg)
+struct EventHelper {
+ EventHelper(ostream& outputFile_arg,
+ OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_arg,
+ const std::string& field_name = "z",
+ float min_v = 0,
+ float max_v = 3.0)
+ : outputFile_(outputFile_arg), octreeEncoder_(octreeEncoder_arg)
{
- pass_.setFilterFieldName (field_name);
- pass_.setFilterLimits (min_v, max_v);
+ pass_.setFilterFieldName(field_name);
+ pass_.setFilterLimits(min_v, max_v);
}
void
- cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
+ cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
{
- if (!outputFile_.fail ())
- {
- PointCloud<PointXYZRGBA>::Ptr cloud_out (new PointCloud<PointXYZRGBA>);
+ if (!outputFile_.fail()) {
+ PointCloud<PointXYZRGBA>::Ptr cloud_out(new PointCloud<PointXYZRGBA>);
// Use a PassThrough filter to clean NaNs and remove data which is not interesting
- pass_.setInputCloud (cloud);
- pass_.filter (*cloud_out);
-
- octreeEncoder_->encodePointCloud (cloud_out, outputFile_);
+ pass_.setInputCloud(cloud);
+ pass_.filter(*cloud_out);
+
+ octreeEncoder_->encodePointCloud(cloud_out, outputFile_);
}
}
void
- run ()
+ run()
{
// create a new grabber for OpenNI devices
- pcl::OpenNIGrabber interface {};
+ pcl::OpenNIGrabber interface{};
// make callback function from member function
- std::function<void
- (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = [this] (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
- {
- cloud_cb_ (cloud);
- };
+ std::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
+ [this](const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
- // connect callback function for desired signal. In this case its a point cloud with color values
- boost::signals2::connection c = interface.registerCallback (f);
+ // connect callback function for desired signal. In this case its a point cloud with
+ // color values
+ boost::signals2::connection c = interface.registerCallback(f);
// start receiving point clouds
- interface.start ();
+ interface.start();
- while (!outputFile_.fail ())
- {
+ while (!outputFile_.fail()) {
std::this_thread::sleep_for(1s);
}
- interface.stop ();
+ interface.stop();
}
pcl::PassThrough<PointXYZRGBA> pass_;
};
int
-main (int argc, char **argv)
+main(int argc, char** argv)
{
OctreePointCloudCompression<PointXYZRGBA>* octreeCoder;
bEnDecode = false;
float min_v = 0.0f, max_v = 3.0f;
- pcl::console::parse_2x_arguments (argc, argv, "-minmax", min_v, max_v, false);
- std::string field_name ("z");
- pcl::console::parse_argument (argc, argv, "-field", field_name);
+ pcl::console::parse_2x_arguments(argc, argv, "-minmax", min_v, max_v, false);
+ std::string field_name("z");
+ pcl::console::parse_argument(argc, argv, "-field", field_name);
- if (pcl::console::find_argument (argc, argv, "-e")>0)
+ if (pcl::console::find_argument(argc, argv, "-e") > 0)
bShowInputCloud = true;
- if (pcl::console::find_argument (argc, argv, "-s")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-s") > 0) {
bEnDecode = true;
bServerFileMode = true;
validArguments = true;
}
- if (pcl::console::parse_argument (argc, argv, "-c", hostName)>0)
- {
+ if (pcl::console::parse_argument(argc, argv, "-c", hostName) > 0) {
bEnDecode = false;
bServerFileMode = true;
validArguments = true;
}
- if (pcl::console::find_argument (argc, argv, "-x")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-x") > 0) {
bEnDecode = true;
bServerFileMode = false;
validArguments = true;
}
- if (pcl::console::find_argument (argc, argv, "-d")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-d") > 0) {
bEnDecode = false;
bServerFileMode = false;
validArguments = true;
}
- if (pcl::console::find_argument (argc, argv, "-t")>0)
+ if (pcl::console::find_argument(argc, argv, "-t") > 0)
showStatistics = true;
- if (pcl::console::find_argument (argc, argv, "-a")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-a") > 0) {
doColorEncoding = true;
compressionProfile = pcl::io::MANUAL_CONFIGURATION;
}
- if (pcl::console::find_argument (argc, argv, "-v")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-v") > 0) {
doVoxelGridDownDownSampling = true;
compressionProfile = pcl::io::MANUAL_CONFIGURATION;
}
- pcl::console::parse_argument (argc, argv, "-f", fileName);
- pcl::console::parse_argument (argc, argv, "-r", pointResolution);
- pcl::console::parse_argument (argc, argv, "-i", iFrameRate);
- pcl::console::parse_argument (argc, argv, "-o", octreeResolution);
- pcl::console::parse_argument (argc, argv, "-b", colorBitResolution);
+ pcl::console::parse_argument(argc, argv, "-f", fileName);
+ pcl::console::parse_argument(argc, argv, "-r", pointResolution);
+ pcl::console::parse_argument(argc, argv, "-i", iFrameRate);
+ pcl::console::parse_argument(argc, argv, "-o", octreeResolution);
+ pcl::console::parse_argument(argc, argv, "-b", colorBitResolution);
std::string profile;
- if (pcl::console::parse_argument (argc, argv, "-p", profile)>0)
- {
+ if (pcl::console::parse_argument(argc, argv, "-p", profile) > 0) {
if (profile == "lowC")
compressionProfile = pcl::io::LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR;
else if (profile == "lowNC")
compressionProfile = pcl::io::LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
else if (profile == "medC")
compressionProfile = pcl::io::MED_RES_OFFLINE_COMPRESSION_WITH_COLOR;
- else if (profile =="medNC")
+ else if (profile == "medNC")
compressionProfile = pcl::io::MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
else if (profile == "highC")
compressionProfile = pcl::io::HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR;
else if (profile == "highNC")
compressionProfile = pcl::io::HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
- else
- {
- print_usage ("Unknown profile parameter..\n");
+ else {
+ print_usage("Unknown profile parameter..\n");
return -1;
}
- if (compressionProfile != MANUAL_CONFIGURATION)
- {
+ if (compressionProfile != MANUAL_CONFIGURATION) {
// apply selected compression profile
// retrieve profile settings
- const pcl::io::configurationProfile_t selectedProfile = pcl::io::compressionProfiles_[compressionProfile];
+ const pcl::io::configurationProfile_t selectedProfile =
+ pcl::io::compressionProfiles_[compressionProfile];
// apply profile settings
pointResolution = selectedProfile.pointResolution;
- octreeResolution = float (selectedProfile.octreeResolution);
+ octreeResolution = float(selectedProfile.octreeResolution);
doVoxelGridDownDownSampling = selectedProfile.doVoxelGridDownSampling;
iFrameRate = selectedProfile.iFrameRate;
doColorEncoding = selectedProfile.doColorEncoding;
}
}
- if (pcl::console::find_argument (argc, argv, "-?")>0)
- {
- print_usage ("");
+ if (pcl::console::find_argument(argc, argv, "-?") > 0) {
+ print_usage("");
return 1;
}
- if (!validArguments)
- {
- print_usage ("Please specify compression mode..\n");
+ if (!validArguments) {
+ print_usage("Please specify compression mode..\n");
return -1;
}
- octreeCoder = new OctreePointCloudCompression<PointXYZRGBA> (compressionProfile, showStatistics, pointResolution,
- octreeResolution, doVoxelGridDownDownSampling, iFrameRate,
- doColorEncoding, static_cast<unsigned char> (colorBitResolution));
-
-
- if (!bServerFileMode)
- {
- if (bEnDecode)
- {
+ octreeCoder = new OctreePointCloudCompression<PointXYZRGBA>(
+ compressionProfile,
+ showStatistics,
+ pointResolution,
+ octreeResolution,
+ doVoxelGridDownDownSampling,
+ iFrameRate,
+ doColorEncoding,
+ static_cast<unsigned char>(colorBitResolution));
+
+ if (!bServerFileMode) {
+ if (bEnDecode) {
// ENCODING
ofstream compressedPCFile;
- compressedPCFile.open (fileName.c_str(), ios::out | ios::trunc | ios::binary);
-
- if (!bShowInputCloud)
- {
- EventHelper v (compressedPCFile, octreeCoder, field_name, min_v, max_v);
- v.run ();
- }
- else
- {
- SimpleOpenNIViewer v (compressedPCFile, octreeCoder);
- v.run ();
- }
+ compressedPCFile.open(fileName.c_str(), ios::out | ios::trunc | ios::binary);
- } else
- {
+ if (!bShowInputCloud) {
+ EventHelper v(compressedPCFile, octreeCoder, field_name, min_v, max_v);
+ v.run();
+ }
+ else {
+ SimpleOpenNIViewer v(compressedPCFile, octreeCoder);
+ v.run();
+ }
+ }
+ else {
// DECODING
ifstream compressedPCFile;
- compressedPCFile.open (fileName.c_str(), ios::in | ios::binary);
- compressedPCFile.seekg (0);
- compressedPCFile.unsetf (ios_base::skipws);
+ compressedPCFile.open(fileName.c_str(), ios::in | ios::binary);
+ compressedPCFile.seekg(0);
+ compressedPCFile.unsetf(ios_base::skipws);
- pcl::visualization::CloudViewer viewer ("PCL Compression Viewer");
+ pcl::visualization::CloudViewer viewer("PCL Compression Viewer");
- while (!compressedPCFile.eof())
- {
- PointCloud<PointXYZRGBA>::Ptr cloudOut (new PointCloud<PointXYZRGBA> ());
- octreeCoder->decodePointCloud ( compressedPCFile, cloudOut);
- viewer.showCloud (cloudOut);
+ while (!compressedPCFile.eof()) {
+ PointCloud<PointXYZRGBA>::Ptr cloudOut(new PointCloud<PointXYZRGBA>());
+ octreeCoder->decodePointCloud(compressedPCFile, cloudOut);
+ viewer.showCloud(cloudOut);
}
}
- } else
- {
- if (bEnDecode)
- {
+ }
+ else {
+ if (bEnDecode) {
// ENCODING
- try
- {
+ try {
boost::asio::io_service io_service;
- tcp::endpoint endpoint (tcp::v4 (), 6666);
- tcp::acceptor acceptor (io_service, endpoint);
+ tcp::endpoint endpoint(tcp::v4(), 6666);
+ tcp::acceptor acceptor(io_service, endpoint);
tcp::iostream socketStream;
std::cout << "Waiting for connection.." << std::endl;
- acceptor.accept (*socketStream.rdbuf ());
+ acceptor.accept(*socketStream.rdbuf());
std::cout << "Connected!" << std::endl;
- if (!bShowInputCloud)
- {
- EventHelper v (socketStream, octreeCoder, field_name, min_v, max_v);
- v.run ();
- }
- else
- {
- SimpleOpenNIViewer v (socketStream, octreeCoder);
- v.run ();
+ if (!bShowInputCloud) {
+ EventHelper v(socketStream, octreeCoder, field_name, min_v, max_v);
+ v.run();
+ }
+ else {
+ SimpleOpenNIViewer v(socketStream, octreeCoder);
+ v.run();
}
std::cout << "Disconnected!" << std::endl;
std::this_thread::sleep_for(3s);
- }
- catch (std::exception& e)
- {
- std::cerr << e.what () << std::endl;
+ } catch (std::exception& e) {
+ std::cerr << e.what() << std::endl;
}
}
- else
- {
+ else {
// DECODING
std::cout << "Connecting to: " << hostName << ".." << std::endl;
- try
- {
- tcp::iostream socketStream (hostName.c_str (), "6666");
+ try {
+ tcp::iostream socketStream(hostName.c_str(), "6666");
std::cout << "Connected!" << std::endl;
- pcl::visualization::CloudViewer viewer ("Decoded Point Cloud - PCL Compression Viewer");
+ pcl::visualization::CloudViewer viewer(
+ "Decoded Point Cloud - PCL Compression Viewer");
- while (!socketStream.fail())
- {
- FPS_CALC ("drawing");
- PointCloud<PointXYZRGBA>::Ptr cloudOut (new PointCloud<PointXYZRGBA> ());
- octreeCoder->decodePointCloud (socketStream, cloudOut);
- viewer.showCloud (cloudOut);
+ while (!socketStream.fail()) {
+ FPS_CALC("drawing");
+ PointCloud<PointXYZRGBA>::Ptr cloudOut(new PointCloud<PointXYZRGBA>());
+ octreeCoder->decodePointCloud(socketStream, cloudOut);
+ viewer.showCloud(cloudOut);
}
- }
- catch (std::exception& e)
- {
- std::cout << "Exception: " << e.what () << std::endl;
+ } catch (std::exception& e) {
+ std::cout << "Exception: " << e.what() << std::endl;
}
}
}
-
- delete (octreeCoder);
- return (0);
+ delete octreeCoder;
+ return 0;
}
-
* Author: Julius Kammerl (julius@kammerl.de)
*/
-#include <thread>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+#include <pcl/compression/organized_pointcloud_compression.h>
+#include <pcl/console/parse.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
-#include <pcl/compression/organized_pointcloud_compression.h>
+#include <boost/asio.hpp>
-#include <iostream>
-#include <vector>
#include <cstdio>
-#include <sstream>
#include <cstdlib>
#include <iostream>
+#include <sstream>
#include <string>
-
-#include <boost/asio.hpp>
+#include <thread>
+#include <vector>
using boost::asio::ip::tcp;
using namespace std::chrono_literals;
char usage[] = "\n"
- " PCL organized point cloud stream compression\n"
- "\n"
- " usage: ./pcl_openni_organized_compression [mode] [parameters]\n"
- "\n"
- " I/O: \n"
- " -f file : file name \n"
- "\n"
- " file compression mode:\n"
- " -x: encode point cloud stream to file\n"
- " -d: decode from file and display point cloud stream\n"
- "\n"
- " network streaming mode:\n"
- " -s : start server on localhost\n"
- " -c host : connect to server and display decoded cloud stream\n"
- "\n"
- " optional compression parameters:\n"
- " -a : enable color coding\n"
- " -t : output statistics\n"
- " -e : show input cloud during encoding\n"
- " -r : raw encoding of disparity maps\n"
- " -g : gray scale conversion\n"
-
- "\n"
- " example:\n"
- " ./pcl_openni_organized_compression -x -t -f pc_compressed.pcc \n"
- "\n";
-
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+ " PCL organized point cloud stream compression\n"
+ "\n"
+ " usage: ./pcl_openni_organized_compression [mode] [parameters]\n"
+ "\n"
+ " I/O: \n"
+ " -f file : file name \n"
+ "\n"
+ " file compression mode:\n"
+ " -x: encode point cloud stream to file\n"
+ " -d: decode from file and display point cloud stream\n"
+ "\n"
+ " network streaming mode:\n"
+ " -s : start server on localhost\n"
+ " -c host : connect to server and display decoded cloud stream\n"
+ "\n"
+ " optional compression parameters:\n"
+ " -a : enable color coding\n"
+ " -t : output statistics\n"
+ " -e : show input cloud during encoding\n"
+ " -r : raw encoding of disparity maps\n"
+ " -g : gray scale conversion\n"
+
+ "\n"
+ " example:\n"
+ " ./pcl_openni_organized_compression -x -t -f pc_compressed.pcc \n"
+ "\n";
+
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
void
-print_usage (const std::string &msg)
+print_usage(const std::string& msg)
{
std::cerr << msg << std::endl;
std::cout << usage << std::endl;
}
-class SimpleOpenNIViewer
-{
- public:
- SimpleOpenNIViewer (ostream& outputFile_arg,
- OrganizedPointCloudCompression<PointXYZRGBA>* octreeEncoder_arg,
- bool doColorEncoding_arg,
- bool bShowStatistics_arg,
- bool bRawImageEncoding_arg,
- bool bGrayScaleConversion_arg,
- int pngLevel_arg = -1) :
- viewer ("Input Point Cloud - PCL Compression Viewer"),
- outputFile_ (outputFile_arg),
- organizedEncoder_ (octreeEncoder_arg),
- doColorEncoding_ (doColorEncoding_arg),
- bShowStatistics_ (bShowStatistics_arg),
- bRawImageEncoding_ (bRawImageEncoding_arg),
- bGrayScaleConversion_(bGrayScaleConversion_arg),
- pngLevel_ (pngLevel_arg)
- {
- }
-
- void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
- {
- if (!viewer.wasStopped ())
- {
- organizedEncoder_->encodePointCloud (cloud, outputFile_, doColorEncoding_,bGrayScaleConversion_,bShowStatistics_, pngLevel_);
+class SimpleOpenNIViewer {
+public:
+ SimpleOpenNIViewer(ostream& outputFile_arg,
+ OrganizedPointCloudCompression<PointXYZRGBA>* octreeEncoder_arg,
+ bool doColorEncoding_arg,
+ bool bShowStatistics_arg,
+ bool bRawImageEncoding_arg,
+ bool bGrayScaleConversion_arg,
+ int pngLevel_arg = -1)
+ : viewer("Input Point Cloud - PCL Compression Viewer")
+ , outputFile_(outputFile_arg)
+ , organizedEncoder_(octreeEncoder_arg)
+ , doColorEncoding_(doColorEncoding_arg)
+ , bShowStatistics_(bShowStatistics_arg)
+ , bRawImageEncoding_(bRawImageEncoding_arg)
+ , bGrayScaleConversion_(bGrayScaleConversion_arg)
+ , pngLevel_(pngLevel_arg)
+ {}
- viewer.showCloud (cloud);
- }
+ void
+ cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
+ {
+ if (!viewer.wasStopped()) {
+ organizedEncoder_->encodePointCloud(cloud,
+ outputFile_,
+ doColorEncoding_,
+ bGrayScaleConversion_,
+ bShowStatistics_,
+ pngLevel_);
+
+ viewer.showCloud(cloud);
}
+ }
+ void
+ run()
+ {
+ if (!bRawImageEncoding_) {
+ // create a new grabber for OpenNI devices
+ pcl::OpenNIGrabber interface{};
- void run ()
- {
-
- if (!bRawImageEncoding_)
- {
- // create a new grabber for OpenNI devices
- pcl::OpenNIGrabber interface {};
-
- // make callback function from member function
- std::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
- [this] (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) { cloud_cb_ (cloud); };
-
- // connect callback function for desired signal. In this case its a point cloud with color values
- boost::signals2::connection c = interface.registerCallback (f);
-
- // start receiving point clouds
- interface.start ();
+ // make callback function from member function
+ std::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
+ [this](const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+ // connect callback function for desired signal. In this case its a point cloud
+ // with color values
+ boost::signals2::connection c = interface.registerCallback(f);
- while (!outputFile_.fail())
- {
- std::this_thread::sleep_for(1s);
- }
+ // start receiving point clouds
+ interface.start();
- interface.stop ();
+ while (!outputFile_.fail()) {
+ std::this_thread::sleep_for(1s);
}
+ interface.stop();
}
+ }
- pcl::visualization::CloudViewer viewer;
- ostream& outputFile_;
-
OrganizedPointCloudCompression<PointXYZRGBA>* organizedEncoder_;
- bool doColorEncoding_;
- bool bShowStatistics_;
- bool bRawImageEncoding_;
- bool bGrayScaleConversion_;
- int pngLevel_;
+ pcl::visualization::CloudViewer viewer;
+ ostream& outputFile_;
+ OrganizedPointCloudCompression<PointXYZRGBA>* organizedEncoder_;
+ bool doColorEncoding_;
+ bool bShowStatistics_;
+ bool bRawImageEncoding_;
+ bool bGrayScaleConversion_;
+ int pngLevel_;
};
-struct EventHelper
-{
- EventHelper (ostream& outputFile_arg,
- OrganizedPointCloudCompression<PointXYZRGBA>* octreeEncoder_arg,
- bool doColorEncoding_arg,
- bool bShowStatistics_arg,
- bool bRawImageEncoding_arg,
- bool bGrayScaleConversion_arg,
- int pngLevel_arg = -1) :
- outputFile_ (outputFile_arg),
- organizedEncoder_ (octreeEncoder_arg),
- doColorEncoding_ (doColorEncoding_arg),
- bShowStatistics_ (bShowStatistics_arg),
- bRawImageEncoding_ (bRawImageEncoding_arg),
- bGrayScaleConversion_(bGrayScaleConversion_arg) ,
- pngLevel_ (pngLevel_arg)
- {
- }
+struct EventHelper {
+ EventHelper(ostream& outputFile_arg,
+ OrganizedPointCloudCompression<PointXYZRGBA>* octreeEncoder_arg,
+ bool doColorEncoding_arg,
+ bool bShowStatistics_arg,
+ bool bRawImageEncoding_arg,
+ bool bGrayScaleConversion_arg,
+ int pngLevel_arg = -1)
+ : outputFile_(outputFile_arg)
+ , organizedEncoder_(octreeEncoder_arg)
+ , doColorEncoding_(doColorEncoding_arg)
+ , bShowStatistics_(bShowStatistics_arg)
+ , bRawImageEncoding_(bRawImageEncoding_arg)
+ , bGrayScaleConversion_(bGrayScaleConversion_arg)
+ , pngLevel_(pngLevel_arg)
+ {}
void
- cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
+ cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
{
- if (!outputFile_.fail ())
- {
- organizedEncoder_->encodePointCloud (cloud, outputFile_, doColorEncoding_, bGrayScaleConversion_, bShowStatistics_, pngLevel_);
+ if (!outputFile_.fail()) {
+ organizedEncoder_->encodePointCloud(cloud,
+ outputFile_,
+ doColorEncoding_,
+ bGrayScaleConversion_,
+ bShowStatistics_,
+ pngLevel_);
}
}
void
- image_callback (const openni_wrapper::Image::Ptr &image,
- const openni_wrapper::DepthImage::Ptr &depth_image, float)
+ image_callback(const openni_wrapper::Image::Ptr& image,
+ const openni_wrapper::DepthImage::Ptr& depth_image,
+ float)
{
std::vector<std::uint16_t> disparity_data;
std::vector<std::uint8_t> rgb_data;
- std::uint32_t width=depth_image->getWidth ();
- std::uint32_t height=depth_image->getHeight ();
-
- disparity_data.resize(width*height);
- depth_image->fillDepthImageRaw (width, height, &disparity_data[0], static_cast<unsigned int> (width * sizeof (std::uint16_t)));
-
- if (image->getEncoding() != openni_wrapper::Image::RGB)
- {
- rgb_data.resize(width*height*3);
- image->fillRGB(width, height, &rgb_data[0], static_cast<unsigned int> (width * sizeof (std::uint8_t) * 3));
+ std::uint32_t width = depth_image->getWidth();
+ std::uint32_t height = depth_image->getHeight();
+
+ disparity_data.resize(width * height);
+ depth_image->fillDepthImageRaw(
+ width,
+ height,
+ &disparity_data[0],
+ static_cast<unsigned int>(width * sizeof(std::uint16_t)));
+
+ if (image->getEncoding() != openni_wrapper::Image::RGB) {
+ rgb_data.resize(width * height * 3);
+ image->fillRGB(width,
+ height,
+ &rgb_data[0],
+ static_cast<unsigned int>(width * sizeof(std::uint8_t) * 3));
}
- organizedEncoder_->encodeRawDisparityMapWithColorImage (disparity_data, rgb_data, width, height, outputFile_, doColorEncoding_, bGrayScaleConversion_, bShowStatistics_, pngLevel_);
-
+ organizedEncoder_->encodeRawDisparityMapWithColorImage(disparity_data,
+ rgb_data,
+ width,
+ height,
+ outputFile_,
+ doColorEncoding_,
+ bGrayScaleConversion_,
+ bShowStatistics_,
+ pngLevel_);
}
void
- run ()
+ run()
{
- if (!bRawImageEncoding_)
- {
+ if (!bRawImageEncoding_) {
// create a new grabber for OpenNI devices
- pcl::OpenNIGrabber interface {};
+ pcl::OpenNIGrabber interface{};
// make callback function from member function
- std::function<void
- (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = [this] (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
- {
- cloud_cb_ (cloud);
- };
+ std::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
+ [this](const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
- // connect callback function for desired signal. In this case its a point cloud with color values
- boost::signals2::connection c = interface.registerCallback (f);
+ // connect callback function for desired signal. In this case its a point cloud
+ // with color values
+ boost::signals2::connection c = interface.registerCallback(f);
// start receiving point clouds
- interface.start ();
+ interface.start();
- while (!outputFile_.fail ())
- {
+ while (!outputFile_.fail()) {
std::this_thread::sleep_for(1s);
}
- interface.stop ();
- } else
- {
+ interface.stop();
+ }
+ else {
pcl::OpenNIGrabber::Mode image_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode;
int depthformat = openni_wrapper::OpenNIDevice::OpenNI_shift_values;
- pcl::OpenNIGrabber grabber ("", pcl::OpenNIGrabber::OpenNI_Default_Mode, image_mode);
+ pcl::OpenNIGrabber grabber(
+ "", pcl::OpenNIGrabber::OpenNI_Default_Mode, image_mode);
// Set the depth output format
- grabber.getDevice ()->setDepthOutputFormat (static_cast<openni_wrapper::OpenNIDevice::DepthMode> (depthformat));
-
- std::function<void (const openni_wrapper::Image::Ptr&,
- const openni_wrapper::DepthImage::Ptr&,
- float) > image_cb = [this] (const openni_wrapper::Image::Ptr& img,
- const openni_wrapper::DepthImage::Ptr& depth,
- float f)
- {
- image_callback (img, depth, f);
- };
- boost::signals2::connection image_connection = grabber.registerCallback (image_cb);
-
- grabber.start ();
- while (!outputFile_.fail())
- {
+ grabber.getDevice()->setDepthOutputFormat(
+ static_cast<openni_wrapper::OpenNIDevice::DepthMode>(depthformat));
+
+ std::function<void(const openni_wrapper::Image::Ptr&,
+ const openni_wrapper::DepthImage::Ptr&,
+ float)>
+ image_cb = [this](const openni_wrapper::Image::Ptr& img,
+ const openni_wrapper::DepthImage::Ptr& depth,
+ float f) { image_callback(img, depth, f); };
+ boost::signals2::connection image_connection = grabber.registerCallback(image_cb);
+
+ grabber.start();
+ while (!outputFile_.fail()) {
std::this_thread::sleep_for(1s);
}
- grabber.stop ();
-
+ grabber.stop();
}
}
};
int
-main (int argc, char **argv)
+main(int argc, char** argv)
{
OrganizedPointCloudCompression<PointXYZRGBA>* organizedCoder;
bRawImageEncoding = false;
bGrayScaleConversion = false;
- if (pcl::console::find_argument (argc, argv, "-e")>0)
+ if (pcl::console::find_argument(argc, argv, "-e") > 0)
bShowInputCloud = true;
- if (pcl::console::find_argument (argc, argv, "-r")>0)
+ if (pcl::console::find_argument(argc, argv, "-r") > 0)
bRawImageEncoding = true;
- if (pcl::console::find_argument (argc, argv, "-g")>0)
+ if (pcl::console::find_argument(argc, argv, "-g") > 0)
bGrayScaleConversion = true;
- if (pcl::console::find_argument (argc, argv, "-s")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-s") > 0) {
bEnDecode = true;
bServerFileMode = true;
validArguments = true;
}
- if (pcl::console::parse_argument (argc, argv, "-c", hostName)>0)
- {
+ if (pcl::console::parse_argument(argc, argv, "-c", hostName) > 0) {
bEnDecode = false;
bServerFileMode = true;
validArguments = true;
}
- if (pcl::console::find_argument (argc, argv, "-a")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-a") > 0) {
doColorEncoding = true;
}
- if (pcl::console::find_argument (argc, argv, "-x")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-x") > 0) {
bEnDecode = true;
bServerFileMode = false;
validArguments = true;
}
- if (pcl::console::find_argument (argc, argv, "-d")>0)
- {
+ if (pcl::console::find_argument(argc, argv, "-d") > 0) {
bEnDecode = false;
bServerFileMode = false;
validArguments = true;
}
- if (pcl::console::find_argument (argc, argv, "-t")>0)
+ if (pcl::console::find_argument(argc, argv, "-t") > 0)
showStatistics = true;
- pcl::console::parse_argument (argc, argv, "-f", fileName);
-
+ pcl::console::parse_argument(argc, argv, "-f", fileName);
- if (pcl::console::find_argument (argc, argv, "-?")>0)
- {
- print_usage ("");
+ if (pcl::console::find_argument(argc, argv, "-?") > 0) {
+ print_usage("");
return 1;
}
- if (!validArguments)
- {
- print_usage ("Please specify compression mode..\n");
+ if (!validArguments) {
+ print_usage("Please specify compression mode..\n");
return -1;
}
- organizedCoder = new OrganizedPointCloudCompression<PointXYZRGBA> ();
+ organizedCoder = new OrganizedPointCloudCompression<PointXYZRGBA>();
-
- if (!bServerFileMode)
- {
- if (bEnDecode)
- {
+ if (!bServerFileMode) {
+ if (bEnDecode) {
// ENCODING
ofstream compressedPCFile;
- compressedPCFile.open (fileName.c_str(), ios::out | ios::trunc | ios::binary);
-
- if (!bShowInputCloud)
- {
- EventHelper v (compressedPCFile, organizedCoder, doColorEncoding, showStatistics, bRawImageEncoding, bGrayScaleConversion);
- v.run ();
- }
- else
- {
- SimpleOpenNIViewer v (compressedPCFile, organizedCoder, doColorEncoding, showStatistics, bRawImageEncoding, bGrayScaleConversion);
- v.run ();
+ compressedPCFile.open(fileName.c_str(), ios::out | ios::trunc | ios::binary);
+
+ if (!bShowInputCloud) {
+ EventHelper v(compressedPCFile,
+ organizedCoder,
+ doColorEncoding,
+ showStatistics,
+ bRawImageEncoding,
+ bGrayScaleConversion);
+ v.run();
}
-
- } else
- {
+ else {
+ SimpleOpenNIViewer v(compressedPCFile,
+ organizedCoder,
+ doColorEncoding,
+ showStatistics,
+ bRawImageEncoding,
+ bGrayScaleConversion);
+ v.run();
+ }
+ }
+ else {
// DECODING
ifstream compressedPCFile;
- compressedPCFile.open (fileName.c_str(), ios::in | ios::binary);
- compressedPCFile.seekg (0);
- compressedPCFile.unsetf (ios_base::skipws);
+ compressedPCFile.open(fileName.c_str(), ios::in | ios::binary);
+ compressedPCFile.seekg(0);
+ compressedPCFile.unsetf(ios_base::skipws);
- pcl::visualization::CloudViewer viewer ("PCL Compression Viewer");
+ pcl::visualization::CloudViewer viewer("PCL Compression Viewer");
- while (!compressedPCFile.eof())
- {
- PointCloud<PointXYZRGBA>::Ptr cloudOut (new PointCloud<PointXYZRGBA> ());
- organizedCoder->decodePointCloud ( compressedPCFile, cloudOut );
- viewer.showCloud (cloudOut);
+ while (!compressedPCFile.eof()) {
+ PointCloud<PointXYZRGBA>::Ptr cloudOut(new PointCloud<PointXYZRGBA>());
+ organizedCoder->decodePointCloud(compressedPCFile, cloudOut);
+ viewer.showCloud(cloudOut);
}
}
- } else
- {
- if (bEnDecode)
- {
+ }
+ else {
+ if (bEnDecode) {
// ENCODING
- try
- {
+ try {
boost::asio::io_service io_service;
- tcp::endpoint endpoint (tcp::v4 (), 6666);
- tcp::acceptor acceptor (io_service, endpoint);
+ tcp::endpoint endpoint(tcp::v4(), 6666);
+ tcp::acceptor acceptor(io_service, endpoint);
tcp::iostream socketStream;
std::cout << "Waiting for connection.." << std::endl;
- acceptor.accept (*socketStream.rdbuf ());
+ acceptor.accept(*socketStream.rdbuf());
std::cout << "Connected!" << std::endl;
- if (!bShowInputCloud)
- {
- EventHelper v (socketStream, organizedCoder, doColorEncoding, showStatistics, bRawImageEncoding, bGrayScaleConversion);
- v.run ();
- }
- else
- {
- SimpleOpenNIViewer v (socketStream, organizedCoder, doColorEncoding, showStatistics, bRawImageEncoding, bGrayScaleConversion);
- v.run ();
+ if (!bShowInputCloud) {
+ EventHelper v(socketStream,
+ organizedCoder,
+ doColorEncoding,
+ showStatistics,
+ bRawImageEncoding,
+ bGrayScaleConversion);
+ v.run();
+ }
+ else {
+ SimpleOpenNIViewer v(socketStream,
+ organizedCoder,
+ doColorEncoding,
+ showStatistics,
+ bRawImageEncoding,
+ bGrayScaleConversion);
+ v.run();
}
std::cout << "Disconnected!" << std::endl;
std::this_thread::sleep_for(3s);
- }
- catch (std::exception& e)
- {
- std::cerr << e.what () << std::endl;
+ } catch (std::exception& e) {
+ std::cerr << e.what() << std::endl;
}
}
- else
- {
+ else {
// DECODING
std::cout << "Connecting to: " << hostName << ".." << std::endl;
- try
- {
- tcp::iostream socketStream (hostName.c_str (), "6666");
+ try {
+ tcp::iostream socketStream(hostName.c_str(), "6666");
std::cout << "Connected!" << std::endl;
- pcl::visualization::CloudViewer viewer ("Decoded Point Cloud - PCL Compression Viewer");
+ pcl::visualization::CloudViewer viewer(
+ "Decoded Point Cloud - PCL Compression Viewer");
- while (!socketStream.fail())
- {
- FPS_CALC ("drawing");
- PointCloud<PointXYZRGBA>::Ptr cloudOut (new PointCloud<PointXYZRGBA> ());
- organizedCoder->decodePointCloud (socketStream, cloudOut);
- viewer.showCloud (cloudOut);
+ while (!socketStream.fail()) {
+ FPS_CALC("drawing");
+ PointCloud<PointXYZRGBA>::Ptr cloudOut(new PointCloud<PointXYZRGBA>());
+ organizedCoder->decodePointCloud(socketStream, cloudOut);
+ viewer.showCloud(cloudOut);
}
- }
- catch (std::exception& e)
- {
- std::cout << "Exception: " << e.what () << std::endl;
+ } catch (std::exception& e) {
+ std::cout << "Exception: " << e.what() << std::endl;
}
}
}
- delete (organizedCoder);
- return (0);
+ delete organizedCoder;
+ return 0;
}
-
*
*/
-
#include <pcl/common/time.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/organized_edge_detection.h>
using PointT = pcl::PointXYZRGBA;
-class OpenNIOrganizedEdgeDetection
-{
- private:
- pcl::visualization::PCLVisualizer::Ptr viewer;
- pcl::PointCloud<PointT> cloud_;
- std::mutex cloud_mutex;
+class OpenNIOrganizedEdgeDetection {
+private:
+ pcl::visualization::PCLVisualizer::Ptr viewer;
+ pcl::PointCloud<PointT> cloud_;
+ std::mutex cloud_mutex;
- public:
- OpenNIOrganizedEdgeDetection ()
- : viewer (new pcl::visualization::PCLVisualizer ("PCL Organized Edge Detection"))
- {
+public:
+ OpenNIOrganizedEdgeDetection()
+ : viewer(new pcl::visualization::PCLVisualizer("PCL Organized Edge Detection"))
+ {}
+ ~OpenNIOrganizedEdgeDetection() {}
+ pcl::visualization::PCLVisualizer::Ptr
+ initCloudViewer(const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ viewer->setSize(640, 480);
+ viewer->addPointCloud<PointT>(cloud, "cloud");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
+ viewer->addCoordinateSystem(0.2f, "global");
+ viewer->initCameraParameters();
+ viewer->registerKeyboardCallback(&OpenNIOrganizedEdgeDetection::keyboard_callback,
+ *this);
+ viewer->resetCameraViewpoint("cloud");
+
+ const int point_size = 2;
+ viewer->addPointCloud<PointT>(cloud, "nan boundary edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
+ point_size,
+ "nan boundary edges");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
+ 0.0f,
+ 0.0f,
+ 1.0f,
+ "nan boundary edges");
+
+ viewer->addPointCloud<PointT>(cloud, "occluding edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
+
+ viewer->addPointCloud<PointT>(cloud, "occluded edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
+
+ viewer->addPointCloud<PointT>(cloud, "high curvature edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
+ point_size,
+ "high curvature edges");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
+ 1.0f,
+ 1.0f,
+ 0.0f,
+ "high curvature edges");
+
+ viewer->addPointCloud<PointT>(cloud, "rgb edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
+
+ return viewer;
+ }
+
+ void
+ keyboard_callback(const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ if (event.keyUp()) {
+ double opacity;
+ switch (event.getKeyCode()) {
+ case '1':
+ viewer->getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY,
+ 1.0 - opacity,
+ "nan boundary edges");
+ break;
+ case '2':
+ viewer->getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY,
+ 1.0 - opacity,
+ "occluding edges");
+ break;
+ case '3':
+ viewer->getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY,
+ 1.0 - opacity,
+ "occluded edges");
+ break;
+ case '4':
+ viewer->getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY,
+ opacity,
+ "high curvature edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY,
+ 1.0 - opacity,
+ "high curvature edges");
+ break;
+ case '5':
+ viewer->getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - opacity, "rgb edges");
+ break;
+ }
}
- ~OpenNIOrganizedEdgeDetection ()
- {
- }
+ }
- pcl::visualization::PCLVisualizer::Ptr
- initCloudViewer (const pcl::PointCloud<PointT>::ConstPtr& cloud)
- {
- viewer->setSize (640, 480);
- viewer->addPointCloud<PointT> (cloud, "cloud");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
- viewer->addCoordinateSystem (0.2f, "global");
- viewer->initCameraParameters ();
- viewer->registerKeyboardCallback (&OpenNIOrganizedEdgeDetection::keyboard_callback, *this);
- viewer->resetCameraViewpoint ("cloud");
-
- const int point_size = 2;
- viewer->addPointCloud<PointT> (cloud, "nan boundary edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
-
- viewer->addPointCloud<PointT> (cloud, "occluding edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
-
- viewer->addPointCloud<PointT> (cloud, "occluded edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
-
- viewer->addPointCloud<PointT> (cloud, "high curvature edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");
-
- viewer->addPointCloud<PointT> (cloud, "rgb edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
-
- return (viewer);
+ void
+ cloud_cb_(const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ if (!viewer->wasStopped()) {
+ cloud_mutex.lock();
+ cloud_ = *cloud;
+ cloud_mutex.unlock();
}
+ }
+
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{};
- void
- keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
- {
- if (event.keyUp())
- {
- double opacity;
- switch (event.getKeyCode())
- {
- case '1':
- viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges");
- break;
- case '2':
- viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges");
- break;
- case '3':
- viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges");
- break;
- case '4':
- viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges");
- break;
- case '5':
- viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges");
- break;
+ std::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f =
+ [this](const pcl::PointCloud<PointT>::ConstPtr& cloud) { cloud_cb_(cloud); };
+
+ // Make and initialize a cloud viewer
+ pcl::PointCloud<PointT>::Ptr init_cloud_ptr(new pcl::PointCloud<PointT>);
+ viewer = initCloudViewer(init_cloud_ptr);
+ boost::signals2::connection c = interface.registerCallback(f);
+
+ interface.start();
+
+ pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+ ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
+ ne.setNormalSmoothingSize(10.0f);
+ ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
+
+ float th_dd = 0.04f;
+ int max_search = 100;
+ pcl::OrganizedEdgeFromRGBNormals<PointT, pcl::Normal, pcl::Label> oed;
+ oed.setDepthDisconThreshold(th_dd);
+ oed.setMaxSearchNeighbors(max_search);
+
+ oed.setEdgeType(oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING |
+ oed.EDGELABEL_OCCLUDED);
+ // oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING |
+ // oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
+
+ pcl::PointCloud<pcl::Label> labels;
+ std::vector<pcl::PointIndices> label_indices;
+
+ while (!viewer->wasStopped()) {
+ viewer->spinOnce();
+
+ if (cloud_mutex.try_lock()) {
+ labels.clear();
+ label_indices.clear();
+
+ double normal_start = pcl::getTime();
+
+ if (oed.getEdgeType() & oed.EDGELABEL_HIGH_CURVATURE) {
+ pcl::PointCloud<pcl::Normal>::Ptr normal_cloud(
+ new pcl::PointCloud<pcl::Normal>);
+ ne.setInputCloud(cloud_.makeShared());
+ ne.compute(*normal_cloud);
+ double normal_end = pcl::getTime();
+ std::cout << "Normal Estimation took " << double(normal_end - normal_start)
+ << std::endl;
+
+ oed.setInputNormals(normal_cloud);
}
- }
- }
- void
- cloud_cb_ (const pcl::PointCloud<PointT>::ConstPtr& cloud)
- {
- if (!viewer->wasStopped ())
- {
- cloud_mutex.lock ();
- cloud_ = *cloud;
- cloud_mutex.unlock ();
- }
- }
+ double oed_start = pcl::getTime();
- void
- run ()
- {
- pcl::OpenNIGrabber interface {};
-
- std::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = [this] (const pcl::PointCloud<PointT>::ConstPtr& cloud) { cloud_cb_ (cloud); };
-
- // Make and initialize a cloud viewer
- pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>);
- viewer = initCloudViewer (init_cloud_ptr);
- boost::signals2::connection c = interface.registerCallback (f);
-
- interface.start ();
-
- pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
- ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
- ne.setNormalSmoothingSize (10.0f);
- ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
-
- float th_dd = 0.04f;
- int max_search = 100;
- pcl::OrganizedEdgeFromRGBNormals<PointT, pcl::Normal, pcl::Label> oed;
- oed.setDepthDisconThreshold (th_dd);
- oed.setMaxSearchNeighbors (max_search);
-
- oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED);
- //oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
-
- pcl::PointCloud<pcl::Label> labels;
- std::vector<pcl::PointIndices> label_indices;
-
- while (!viewer->wasStopped ())
- {
- viewer->spinOnce ();
-
- if (cloud_mutex.try_lock ())
- {
- labels.clear ();
- label_indices.clear ();
-
- double normal_start = pcl::getTime ();
-
- if (oed.getEdgeType () & oed.EDGELABEL_HIGH_CURVATURE)
- {
- pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
- ne.setInputCloud (cloud_.makeShared ());
- ne.compute (*normal_cloud);
- double normal_end = pcl::getTime ();
- std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
-
- oed.setInputNormals (normal_cloud);
- }
-
- double oed_start = pcl::getTime ();
-
- oed.setInputCloud (cloud_.makeShared ());
- oed.compute (labels, label_indices);
-
- double oed_end = pcl::getTime ();
- std::cout << "Edge Detection took " << double (oed_end - oed_start) << std::endl;
- std::cout << "Frame took " << double (oed_end - normal_start) << std::endl;
-
- // Make gray point cloud
- for (auto &point : cloud_.points)
- {
- std::uint8_t gray = std::uint8_t((point.r + point.g + point.b)/3);
- point.r = point.g = point.b = gray;
- }
-
- // Show the gray point cloud
- if (!viewer->updatePointCloud (cloud_.makeShared (), "cloud"))
- viewer->addPointCloud (cloud_.makeShared (), "cloud");
-
- // Show edges
- pcl::PointCloud<PointT>::Ptr occluding_edges (new pcl::PointCloud<PointT>),
- occluded_edges (new pcl::PointCloud<PointT>),
- nan_boundary_edges (new pcl::PointCloud<PointT>),
- high_curvature_edges (new pcl::PointCloud<PointT>),
- rgb_edges (new pcl::PointCloud<PointT>);
-
- pcl::copyPointCloud (cloud_, label_indices[0].indices, *nan_boundary_edges);
- pcl::copyPointCloud (cloud_, label_indices[1].indices, *occluding_edges);
- pcl::copyPointCloud (cloud_, label_indices[2].indices, *occluded_edges);
- pcl::copyPointCloud (cloud_, label_indices[3].indices, *high_curvature_edges);
- pcl::copyPointCloud (cloud_, label_indices[4].indices, *rgb_edges);
-
- if (!viewer->updatePointCloud<PointT> (nan_boundary_edges, "nan boundary edges"))
- viewer->addPointCloud<PointT> (nan_boundary_edges, "nan boundary edges");
-
- if (!viewer->updatePointCloud<PointT> (occluding_edges, "occluding edges"))
- viewer->addPointCloud<PointT> (occluding_edges, "occluding edges");
-
- if (!viewer->updatePointCloud<PointT> (occluded_edges, "occluded edges"))
- viewer->addPointCloud<PointT> (occluded_edges, "occluded edges");
-
- if (!viewer->updatePointCloud<PointT> (high_curvature_edges, "high curvature edges"))
- viewer->addPointCloud<PointT> (high_curvature_edges, "high curvature edges");
-
- if (!viewer->updatePointCloud<PointT> (rgb_edges, "rgb edges"))
- viewer->addPointCloud<PointT> (rgb_edges, "rgb edges");
-
- cloud_mutex.unlock ();
+ oed.setInputCloud(cloud_.makeShared());
+ oed.compute(labels, label_indices);
+
+ double oed_end = pcl::getTime();
+ std::cout << "Edge Detection took " << double(oed_end - oed_start) << std::endl;
+ std::cout << "Frame took " << double(oed_end - normal_start) << std::endl;
+
+ // Make gray point cloud
+ for (auto& point : cloud_.points) {
+ std::uint8_t gray = std::uint8_t((point.r + point.g + point.b) / 3);
+ point.r = point.g = point.b = gray;
}
- }
- interface.stop ();
+ // Show the gray point cloud
+ if (!viewer->updatePointCloud(cloud_.makeShared(), "cloud"))
+ viewer->addPointCloud(cloud_.makeShared(), "cloud");
+
+ // Show edges
+ pcl::PointCloud<PointT>::Ptr occluding_edges(new pcl::PointCloud<PointT>),
+ occluded_edges(new pcl::PointCloud<PointT>),
+ nan_boundary_edges(new pcl::PointCloud<PointT>),
+ high_curvature_edges(new pcl::PointCloud<PointT>),
+ rgb_edges(new pcl::PointCloud<PointT>);
+
+ pcl::copyPointCloud(cloud_, label_indices[0].indices, *nan_boundary_edges);
+ pcl::copyPointCloud(cloud_, label_indices[1].indices, *occluding_edges);
+ pcl::copyPointCloud(cloud_, label_indices[2].indices, *occluded_edges);
+ pcl::copyPointCloud(cloud_, label_indices[3].indices, *high_curvature_edges);
+ pcl::copyPointCloud(cloud_, label_indices[4].indices, *rgb_edges);
+
+ if (!viewer->updatePointCloud<PointT>(nan_boundary_edges, "nan boundary edges"))
+ viewer->addPointCloud<PointT>(nan_boundary_edges, "nan boundary edges");
+
+ if (!viewer->updatePointCloud<PointT>(occluding_edges, "occluding edges"))
+ viewer->addPointCloud<PointT>(occluding_edges, "occluding edges");
+
+ if (!viewer->updatePointCloud<PointT>(occluded_edges, "occluded edges"))
+ viewer->addPointCloud<PointT>(occluded_edges, "occluded edges");
+
+ if (!viewer->updatePointCloud<PointT>(high_curvature_edges,
+ "high curvature edges"))
+ viewer->addPointCloud<PointT>(high_curvature_edges, "high curvature edges");
+
+ if (!viewer->updatePointCloud<PointT>(rgb_edges, "rgb edges"))
+ viewer->addPointCloud<PointT>(rgb_edges, "rgb edges");
+
+ cloud_mutex.unlock();
+ }
}
+
+ interface.stop();
+ }
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
std::string arg;
if (argc > 1)
- arg = std::string (argv[1]);
-
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ arg = std::string(argv[1]);
+
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
-
+
+ // clang-format off
std::cout << "Press following keys to enable/disable the different edge types:" << std::endl;
std::cout << "<1> EDGELABEL_NAN_BOUNDARY edge" << std::endl;
std::cout << "<2> EDGELABEL_OCCLUDING edge" << std::endl;
//std::cout << "<4> EDGELABEL_HIGH_CURVATURE edge" << std::endl;
//std::cout << "<5> EDGELABEL_RGB_CANNY edge" << std::endl;
std::cout << "<Q,q> quit" << std::endl;
-
-
- pcl::OpenNIGrabber grabber ("");
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb> ())
- {
+ // clang-format on
+ pcl::OpenNIGrabber grabber("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb>()) {
OpenNIOrganizedEdgeDetection app;
- app.run ();
+ app.run();
}
else
- PCL_ERROR ("The input device does not provide a PointXYZRGBA mode.\n");
+ PCL_ERROR("The input device does not provide a PointXYZRGBA mode.\n");
- return (0);
+ return 0;
}
*
*/
-#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/io/io.h>
#include <pcl/common/time.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
-#include <pcl/ModelCoefficients.h>
-#include <pcl/segmentation/planar_region.h>
-#include <pcl/segmentation/organized_multi_plane_segmentation.h>
-#include <pcl/segmentation/organized_connected_component_segmentation.h>
#include <pcl/filters/extract_indices.h>
+#include <pcl/io/io.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/segmentation/organized_connected_component_segmentation.h>
+#include <pcl/segmentation/organized_multi_plane_segmentation.h>
+#include <pcl/segmentation/planar_region.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/ModelCoefficients.h>
#include <mutex>
-
using PointT = pcl::PointXYZRGBA;
-class OpenNIOrganizedMultiPlaneSegmentation
-{
- private:
- pcl::visualization::PCLVisualizer::Ptr viewer;
- pcl::PointCloud<PointT>::ConstPtr prev_cloud;
- std::mutex cloud_mutex;
-
- public:
- OpenNIOrganizedMultiPlaneSegmentation ()
- {
-
+class OpenNIOrganizedMultiPlaneSegmentation {
+private:
+ pcl::visualization::PCLVisualizer::Ptr viewer;
+ pcl::PointCloud<PointT>::ConstPtr prev_cloud;
+ std::mutex cloud_mutex;
+
+public:
+ OpenNIOrganizedMultiPlaneSegmentation() {}
+ ~OpenNIOrganizedMultiPlaneSegmentation() {}
+
+ pcl::visualization::PCLVisualizer::Ptr
+ cloudViewer(const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ pcl::visualization::PCLVisualizer::Ptr viewer(
+ new pcl::visualization::PCLVisualizer("Viewer"));
+ viewer->setBackgroundColor(0, 0, 0);
+ pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color(
+ cloud, 0, 255, 0);
+ viewer->addPointCloud<PointT>(cloud, single_color, "cloud");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud");
+ viewer->addCoordinateSystem(1.0, "global");
+ viewer->initCameraParameters();
+ return viewer;
+ }
+
+ void
+ cloud_cb_(const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ if (!viewer->wasStopped()) {
+ cloud_mutex.lock();
+ prev_cloud = cloud;
+ cloud_mutex.unlock();
}
- ~OpenNIOrganizedMultiPlaneSegmentation ()
- {
+ }
+
+ void
+ removePreviousDataFromScreen(std::size_t prev_models_size)
+ {
+ char name[1024];
+ for (std::size_t i = 0; i < prev_models_size; i++) {
+ sprintf(name, "normal_%lu", i);
+ viewer->removeShape(name);
+
+ sprintf(name, "plane_%02zu", i);
+ viewer->removePointCloud(name);
}
-
- pcl::visualization::PCLVisualizer::Ptr
- cloudViewer (const pcl::PointCloud<PointT>::ConstPtr& cloud)
- {
- pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("Viewer"));
- viewer->setBackgroundColor (0, 0, 0);
- pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
- viewer->addPointCloud<PointT> (cloud, single_color, "cloud");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud");
- viewer->addCoordinateSystem (1.0, "global");
- viewer->initCameraParameters ();
- return (viewer);
- }
-
- void
- cloud_cb_ (const pcl::PointCloud<PointT>::ConstPtr& cloud)
- {
- if (!viewer->wasStopped ())
- {
- cloud_mutex.lock ();
- prev_cloud = cloud;
- cloud_mutex.unlock ();
- }
- }
-
- void
- removePreviousDataFromScreen (std::size_t prev_models_size)
- {
- char name[1024];
- for (std::size_t i = 0; i < prev_models_size; i++)
- {
- sprintf (name, "normal_%lu", i);
- viewer->removeShape (name);
-
- sprintf (name, "plane_%02zu", i);
- viewer->removePointCloud (name);
- }
- }
-
- void
- run ()
- {
- pcl::OpenNIGrabber interface {};
-
- std::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = [this] (const pcl::PointCloud<PointT>::ConstPtr& cloud) { cloud_cb_ (cloud); };
-
- //make a viewer
- pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>);
- viewer = cloudViewer (init_cloud_ptr);
- boost::signals2::connection c = interface.registerCallback (f);
-
- interface.start ();
-
- unsigned char red [6] = {255, 0, 0, 255, 255, 0};
- unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
- unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
-
- pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
- ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
- ne.setMaxDepthChangeFactor (0.03f);
- ne.setNormalSmoothingSize (20.0f);
-
- pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
- mps.setMinInliers (10000);
- mps.setAngularThreshold (0.017453 * 2.0); //3 degrees
- mps.setDistanceThreshold (0.02); //2cm
-
- std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
- pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
- std::size_t prev_models_size = 0;
- char name[1024];
-
- while (!viewer->wasStopped ())
- {
- viewer->spinOnce (100);
-
- if (prev_cloud && cloud_mutex.try_lock ())
- {
- regions.clear ();
- pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
- double normal_start = pcl::getTime ();
- ne.setInputCloud (prev_cloud);
- ne.compute (*normal_cloud);
- double normal_end = pcl::getTime ();
- std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
-
- double plane_extract_start = pcl::getTime ();
- mps.setInputNormals (normal_cloud);
- mps.setInputCloud (prev_cloud);
- mps.segmentAndRefine (regions);
- double plane_extract_end = pcl::getTime ();
- std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl;
- std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
-
- pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
-
- if (!viewer->updatePointCloud<PointT> (prev_cloud, "cloud"))
- viewer->addPointCloud<PointT> (prev_cloud, "cloud");
-
- removePreviousDataFromScreen (prev_models_size);
- //Draw Visualization
- for (std::size_t i = 0; i < regions.size (); i++)
- {
- Eigen::Vector3f centroid = regions[i].getCentroid ();
- Eigen::Vector4f model = regions[i].getCoefficients ();
- pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
- pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
- centroid[1] + (0.5f * model[1]),
- centroid[2] + (0.5f * model[2]));
- sprintf (name, "normal_%lu", i);
- viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name);
-
- contour->points = regions[i].getContour ();
- sprintf (name, "plane_%02zu", i);
- pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
- viewer->addPointCloud (contour, color, name);
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
- }
- prev_models_size = regions.size ();
- cloud_mutex.unlock ();
+ }
+
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{};
+
+ std::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f =
+ [this](const pcl::PointCloud<PointT>::ConstPtr& cloud) { cloud_cb_(cloud); };
+
+ // make a viewer
+ pcl::PointCloud<PointT>::Ptr init_cloud_ptr(new pcl::PointCloud<PointT>);
+ viewer = cloudViewer(init_cloud_ptr);
+ boost::signals2::connection c = interface.registerCallback(f);
+
+ interface.start();
+
+ unsigned char red[6] = {255, 0, 0, 255, 255, 0};
+ unsigned char grn[6] = {0, 255, 0, 255, 0, 255};
+ unsigned char blu[6] = {0, 0, 255, 0, 255, 255};
+
+ pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+ ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
+ ne.setMaxDepthChangeFactor(0.03f);
+ ne.setNormalSmoothingSize(20.0f);
+
+ pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
+ mps.setMinInliers(10000);
+ mps.setAngularThreshold(0.017453 * 2.0); // 3 degrees
+ mps.setDistanceThreshold(0.02); // 2cm
+
+ std::vector<pcl::PlanarRegion<PointT>,
+ Eigen::aligned_allocator<pcl::PlanarRegion<PointT>>>
+ regions;
+ pcl::PointCloud<PointT>::Ptr contour(new pcl::PointCloud<PointT>);
+ std::size_t prev_models_size = 0;
+ char name[1024];
+
+ while (!viewer->wasStopped()) {
+ viewer->spinOnce(100);
+
+ if (prev_cloud && cloud_mutex.try_lock()) {
+ regions.clear();
+ pcl::PointCloud<pcl::Normal>::Ptr normal_cloud(
+ new pcl::PointCloud<pcl::Normal>);
+ double normal_start = pcl::getTime();
+ ne.setInputCloud(prev_cloud);
+ ne.compute(*normal_cloud);
+ double normal_end = pcl::getTime();
+ std::cout << "Normal Estimation took " << double(normal_end - normal_start)
+ << std::endl;
+
+ double plane_extract_start = pcl::getTime();
+ mps.setInputNormals(normal_cloud);
+ mps.setInputCloud(prev_cloud);
+ mps.segmentAndRefine(regions);
+ double plane_extract_end = pcl::getTime();
+ std::cout << "Plane extraction took "
+ << double(plane_extract_end - plane_extract_start) << std::endl;
+ std::cout << "Frame took " << double(plane_extract_end - normal_start)
+ << std::endl;
+
+ pcl::PointCloud<PointT>::Ptr cluster(new pcl::PointCloud<PointT>);
+
+ if (!viewer->updatePointCloud<PointT>(prev_cloud, "cloud"))
+ viewer->addPointCloud<PointT>(prev_cloud, "cloud");
+
+ removePreviousDataFromScreen(prev_models_size);
+ // Draw Visualization
+ for (std::size_t i = 0; i < regions.size(); i++) {
+ Eigen::Vector3f centroid = regions[i].getCentroid();
+ Eigen::Vector4f model = regions[i].getCoefficients();
+ pcl::PointXYZ pt1 = pcl::PointXYZ(centroid[0], centroid[1], centroid[2]);
+ pcl::PointXYZ pt2 = pcl::PointXYZ(centroid[0] + (0.5f * model[0]),
+ centroid[1] + (0.5f * model[1]),
+ centroid[2] + (0.5f * model[2]));
+ sprintf(name, "normal_%lu", i);
+ viewer->addArrow(pt2, pt1, 1.0, 0, 0, false, name);
+
+ contour->points = regions[i].getContour();
+ sprintf(name, "plane_%02zu", i);
+ pcl::visualization::PointCloudColorHandlerCustom<PointT> color(
+ contour, red[i], grn[i], blu[i]);
+ viewer->addPointCloud(contour, color, name);
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
}
+ prev_models_size = regions.size();
+ cloud_mutex.unlock();
}
-
- interface.stop ();
}
+
+ interface.stop();
+ }
};
int
-main ()
+main()
{
OpenNIOrganizedMultiPlaneSegmentation multi_plane_app;
- multi_plane_app.run ();
+ multi_plane_app.run();
return 0;
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
-#include <thread>
-
#include <pcl/apps/openni_passthrough.h>
-// QT
+#include <pcl/console/parse.h>
+
#include <QApplication>
-#include <QMutexLocker>
#include <QEvent>
+#include <QMutexLocker>
#include <QObject>
-// PCL
-#include <pcl/console/parse.h>
#include <vtkRenderWindow.h>
+#include <thread>
+
using namespace std::chrono_literals;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-OpenNIPassthrough::OpenNIPassthrough (pcl::OpenNIGrabber& grabber)
- : grabber_(grabber)
- , ui_ (new Ui::MainWindow)
- , vis_timer_ (new QTimer (this))
+OpenNIPassthrough::OpenNIPassthrough(pcl::OpenNIGrabber& grabber)
+: grabber_(grabber), ui_(new Ui::MainWindow), vis_timer_(new QTimer(this))
{
// Create a timer and fire it up every 5ms
- vis_timer_->start (5);
+ vis_timer_->start(5);
- connect (vis_timer_, SIGNAL (timeout ()), this, SLOT (timeoutSlot ()));
+ connect(vis_timer_, SIGNAL(timeout()), this, SLOT(timeoutSlot()));
- ui_->setupUi (this);
+ ui_->setupUi(this);
- this->setWindowTitle ("PCL OpenNI PassThrough Viewer");
- vis_.reset (new pcl::visualization::PCLVisualizer ("", false));
- ui_->qvtk_widget->SetRenderWindow (vis_->getRenderWindow ());
- vis_->setupInteractor (ui_->qvtk_widget->GetInteractor (), ui_->qvtk_widget->GetRenderWindow ());
- vis_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
- ui_->qvtk_widget->update ();
+ this->setWindowTitle("PCL OpenNI PassThrough Viewer");
+ vis_.reset(new pcl::visualization::PCLVisualizer("", false));
+ ui_->qvtk_widget->SetRenderWindow(vis_->getRenderWindow());
+ vis_->setupInteractor(ui_->qvtk_widget->GetInteractor(),
+ ui_->qvtk_widget->GetRenderWindow());
+ vis_->getInteractorStyle()->setKeyboardModifier(
+ pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+ ui_->qvtk_widget->update();
// Start the OpenNI data acquision
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb (cloud); };
- boost::signals2::connection c = grabber_.registerCallback (f);
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
+ boost::signals2::connection c = grabber_.registerCallback(f);
- grabber_.start ();
+ grabber_.start();
// Set defaults
- pass_.setFilterFieldName ("z");
- pass_.setFilterLimits (0.5, 5.0);
-
- ui_->fieldValueSlider->setRange (5, 50);
- ui_->fieldValueSlider->setValue (50);
- connect (ui_->fieldValueSlider, SIGNAL (valueChanged (int)), this, SLOT (adjustPassThroughValues (int)));
+ pass_.setFilterFieldName("z");
+ pass_.setFilterLimits(0.5, 5.0);
+
+ ui_->fieldValueSlider->setRange(5, 50);
+ ui_->fieldValueSlider->setValue(50);
+ connect(ui_->fieldValueSlider,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(adjustPassThroughValues(int)));
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////
void
-OpenNIPassthrough::cloud_cb (const CloudConstPtr& cloud)
+OpenNIPassthrough::cloud_cb(const CloudConstPtr& cloud)
{
- QMutexLocker locker (&mtx_);
- FPS_CALC ("computation");
+ QMutexLocker locker(&mtx_);
+ FPS_CALC("computation");
// Computation goes here
- cloud_pass_.reset (new Cloud);
- pass_.setInputCloud (cloud);
- pass_.filter (*cloud_pass_);
+ cloud_pass_.reset(new Cloud);
+ pass_.setInputCloud(cloud);
+ pass_.filter(*cloud_pass_);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////
void
-OpenNIPassthrough::timeoutSlot ()
+OpenNIPassthrough::timeoutSlot()
{
- if (!cloud_pass_)
- {
+ if (!cloud_pass_) {
std::this_thread::sleep_for(1ms);
return;
}
CloudPtr temp_cloud;
{
- QMutexLocker locker (&mtx_);
- temp_cloud.swap (cloud_pass_);
+ QMutexLocker locker(&mtx_);
+ temp_cloud.swap(cloud_pass_);
}
// Add to the 3D viewer
- if (!vis_->updatePointCloud (temp_cloud, "cloud_pass"))
- {
- vis_->addPointCloud (temp_cloud, "cloud_pass");
- vis_->resetCameraViewpoint ("cloud_pass");
+ if (!vis_->updatePointCloud(temp_cloud, "cloud_pass")) {
+ vis_->addPointCloud(temp_cloud, "cloud_pass");
+ vis_->resetCameraViewpoint("cloud_pass");
}
- FPS_CALC ("visualization");
- ui_->qvtk_widget->update ();
+ FPS_CALC("visualization");
+ ui_->qvtk_widget->update();
}
-int
-main (int argc, char ** argv)
+int
+main(int argc, char** argv)
{
// Initialize QT
- QApplication app (argc, argv);
+ QApplication app(argc, argv);
// Open the first available camera
- pcl::OpenNIGrabber grabber ("#1");
+ pcl::OpenNIGrabber grabber("#1");
// Check if an RGB stream is provided
- if (!grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb> ())
- {
- PCL_ERROR ("Device #1 does not provide an RGB stream!\n");
- return (-1);
+ if (!grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb>()) {
+ PCL_ERROR("Device #1 does not provide an RGB stream!\n");
+ return -1;
}
- OpenNIPassthrough v (grabber);
- v.show ();
- return (QApplication::exec ());
+ OpenNIPassthrough v(grabber);
+ v.show();
+ return QApplication::exec();
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/console/parse.h>
+#include <pcl/filters/project_inliers.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/openni_camera/openni_driver.h>
+#include <pcl/io/openni_grabber.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/project_inliers.h>
#include <pcl/surface/convex_hull.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
template <typename PointType>
-class OpenNIPlanarSegmentation
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNIPlanarSegmentation (const std::string& device_id = "", double threshold = 0.01)
- : viewer ("PCL OpenNI Planar Hull Segmentation Viewer"),
- device_id_ (device_id)
- {
- grid_.setFilterFieldName ("z");
- grid_.setFilterLimits (0.0, 3.0);
- grid_.setLeafSize (0.01f, 0.01f, 0.01f);
-
- seg_.setOptimizeCoefficients (true);
- seg_.setModelType (pcl::SACMODEL_PLANE);
- seg_.setMethodType (pcl::SAC_RANSAC);
- seg_.setMaxIterations (1000);
- seg_.setDistanceThreshold (threshold);
-
- proj_.setModelType (pcl::SACMODEL_PLANE);
- }
+class OpenNIPlanarSegmentation {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNIPlanarSegmentation(const std::string& device_id = "", double threshold = 0.01)
+ : viewer("PCL OpenNI Planar Hull Segmentation Viewer"), device_id_(device_id)
+ {
+ grid_.setFilterFieldName("z");
+ grid_.setFilterLimits(0.0, 3.0);
+ grid_.setLeafSize(0.01f, 0.01f, 0.01f);
- void
- cloud_cb_ (const CloudConstPtr& cloud)
- {
- set (cloud);
- }
+ seg_.setOptimizeCoefficients(true);
+ seg_.setModelType(pcl::SACMODEL_PLANE);
+ seg_.setMethodType(pcl::SAC_RANSAC);
+ seg_.setMaxIterations(1000);
+ seg_.setDistanceThreshold(threshold);
- void
- set (const CloudConstPtr& cloud)
- {
- //lock while we set our cloud;
- std::lock_guard<std::mutex> lock (mtx_);
- cloud_ = cloud;
- }
+ proj_.setModelType(pcl::SACMODEL_PLANE);
+ }
+
+ void
+ cloud_cb_(const CloudConstPtr& cloud)
+ {
+ set(cloud);
+ }
- CloudPtr
- get ()
- {
- //lock while we swap our cloud and reset it.
- std::lock_guard<std::mutex> lock (mtx_);
- CloudPtr temp_cloud (new Cloud);
- CloudPtr temp_cloud2 (new Cloud);
+ void
+ set(const CloudConstPtr& cloud)
+ {
+ // lock while we set our cloud;
+ std::lock_guard<std::mutex> lock(mtx_);
+ cloud_ = cloud;
+ }
- grid_.setInputCloud (cloud_);
- grid_.filter (*temp_cloud);
+ CloudPtr
+ get()
+ {
+ // lock while we swap our cloud and reset it.
+ std::lock_guard<std::mutex> lock(mtx_);
+ CloudPtr temp_cloud(new Cloud);
+ CloudPtr temp_cloud2(new Cloud);
- pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
- pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
+ grid_.setInputCloud(cloud_);
+ grid_.filter(*temp_cloud);
- seg_.setInputCloud (temp_cloud);
- seg_.segment (*inliers, *coefficients);
+ pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
- // Project the model inliers
- proj_.setInputCloud (temp_cloud);
- proj_.setModelCoefficients (coefficients);
- proj_.filter (*temp_cloud2);
+ seg_.setInputCloud(temp_cloud);
+ seg_.segment(*inliers, *coefficients);
- // Create a Convex Hull representation of the projected inliers
- chull_.setInputCloud (temp_cloud2);
- chull_.reconstruct (*temp_cloud);
+ // Project the model inliers
+ proj_.setInputCloud(temp_cloud);
+ proj_.setModelCoefficients(coefficients);
+ proj_.filter(*temp_cloud2);
- return (temp_cloud);
- }
+ // Create a Convex Hull representation of the projected inliers
+ chull_.setInputCloud(temp_cloud2);
+ chull_.reconstruct(*temp_cloud);
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
-
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb_ (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
-
- interface.start ();
-
- while (!viewer.wasStopped ())
- {
- if (cloud_)
- {
- //the call to get() sets the cloud_ to null;
- viewer.showCloud (get ());
- }
- }
+ return temp_cloud;
+ }
+
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
+
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- interface.stop ();
+ interface.start();
+
+ while (!viewer.wasStopped()) {
+ if (cloud_) {
+ // the call to get() sets the cloud_ to null;
+ viewer.showCloud(get());
+ }
}
- pcl::visualization::CloudViewer viewer;
- pcl::VoxelGrid<PointType> grid_;
- pcl::SACSegmentation<PointType> seg_;
- pcl::ProjectInliers<PointType> proj_;
- pcl::ConvexHull<PointType> chull_;
+ interface.stop();
+ }
+
+ pcl::visualization::CloudViewer viewer;
+ pcl::VoxelGrid<PointType> grid_;
+ pcl::SACSegmentation<PointType> seg_;
+ pcl::ProjectInliers<PointType> proj_;
+ pcl::ConvexHull<PointType> chull_;
- std::string device_id_;
- std::mutex mtx_;
- CloudConstPtr cloud_;
+ std::string device_id_;
+ std::mutex mtx_;
+ CloudConstPtr cloud_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n"
- << "where options are:\n -thresh X :: set the planar segmentation threshold (default: 0.5)\n";
+ << "where options are:\n -thresh X :: set the planar "
+ "segmentation threshold (default: 0.5)\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (wotks only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
std::cout << "No devices connected." << std::endl;
}
-int
-main (int argc, char ** argv)
+int
+main(int argc, char** argv)
{
- if (argc < 2)
- {
- usage (argv);
+ if (argc < 2) {
+ usage(argv);
return 1;
}
- std::string arg (argv[1]);
-
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ std::string arg(argv[1]);
+
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
double threshold = 0.05;
- pcl::console::parse_argument (argc, argv, "-thresh", threshold);
+ pcl::console::parse_argument(argc, argv, "-thresh", threshold);
- pcl::OpenNIGrabber grabber (arg);
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- OpenNIPlanarSegmentation<pcl::PointXYZRGBA> v (arg, threshold);
- v.run ();
+ pcl::OpenNIGrabber grabber(arg);
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ OpenNIPlanarSegmentation<pcl::PointXYZRGBA> v(arg, threshold);
+ v.run();
}
- else
- {
- OpenNIPlanarSegmentation<pcl::PointXYZ> v (arg, threshold);
- v.run ();
+ else {
+ OpenNIPlanarSegmentation<pcl::PointXYZ> v(arg, threshold);
+ v.run();
}
- return (0);
+ return 0;
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/console/parse.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/openni_camera/openni_driver.h>
+#include <pcl/io/openni_grabber.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/extract_indices.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
-
template <typename PointType>
-class OpenNIPlanarSegmentation
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNIPlanarSegmentation (const std::string& device_id = "", double threshold = 0.01)
- : viewer ("PCL OpenNI Planar Segmentation Viewer"),
- device_id_ (device_id)
- {
- grid_.setFilterFieldName ("z");
- grid_.setFilterLimits (0.0f, 3.0f);
- grid_.setLeafSize (0.01f, 0.01f, 0.01f);
-
- seg_.setOptimizeCoefficients (true);
- seg_.setModelType (pcl::SACMODEL_PLANE);
- seg_.setMethodType (pcl::SAC_RANSAC);
- seg_.setMaxIterations (1000);
- seg_.setDistanceThreshold (threshold);
-
- extract_.setNegative (false);
- }
+class OpenNIPlanarSegmentation {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNIPlanarSegmentation(const std::string& device_id = "", double threshold = 0.01)
+ : viewer("PCL OpenNI Planar Segmentation Viewer"), device_id_(device_id)
+ {
+ grid_.setFilterFieldName("z");
+ grid_.setFilterLimits(0.0f, 3.0f);
+ grid_.setLeafSize(0.01f, 0.01f, 0.01f);
- void
- cloud_cb_ (const CloudConstPtr& cloud)
- {
- set (cloud);
- }
+ seg_.setOptimizeCoefficients(true);
+ seg_.setModelType(pcl::SACMODEL_PLANE);
+ seg_.setMethodType(pcl::SAC_RANSAC);
+ seg_.setMaxIterations(1000);
+ seg_.setDistanceThreshold(threshold);
- void
- set (const CloudConstPtr& cloud)
- {
- //lock while we set our cloud;
- std::lock_guard<std::mutex> lock (mtx_);
- cloud_ = cloud;
- }
+ extract_.setNegative(false);
+ }
- CloudPtr
- get ()
- {
- //lock while we swap our cloud and reset it.
- std::lock_guard<std::mutex> lock (mtx_);
- CloudPtr temp_cloud (new Cloud);
- CloudPtr temp_cloud2 (new Cloud);
+ void
+ cloud_cb_(const CloudConstPtr& cloud)
+ {
+ set(cloud);
+ }
- grid_.setInputCloud (cloud_);
- grid_.filter (*temp_cloud);
+ void
+ set(const CloudConstPtr& cloud)
+ {
+ // lock while we set our cloud;
+ std::lock_guard<std::mutex> lock(mtx_);
+ cloud_ = cloud;
+ }
- pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
- pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
+ CloudPtr
+ get()
+ {
+ // lock while we swap our cloud and reset it.
+ std::lock_guard<std::mutex> lock(mtx_);
+ CloudPtr temp_cloud(new Cloud);
+ CloudPtr temp_cloud2(new Cloud);
- seg_.setInputCloud (temp_cloud);
- seg_.segment (*inliers, *coefficients);
+ grid_.setInputCloud(cloud_);
+ grid_.filter(*temp_cloud);
- extract_.setInputCloud (temp_cloud);
- extract_.setIndices (inliers);
- extract_.filter (*temp_cloud2);
+ pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
- return (temp_cloud2);
- }
+ seg_.setInputCloud(temp_cloud);
+ seg_.segment(*inliers, *coefficients);
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
-
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb_ (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
-
- interface.start ();
-
- while (!viewer.wasStopped ())
- {
- if (cloud_)
- {
- //the call to get() sets the cloud_ to null;
- viewer.showCloud (get ());
- }
- }
+ extract_.setInputCloud(temp_cloud);
+ extract_.setIndices(inliers);
+ extract_.filter(*temp_cloud2);
+
+ return temp_cloud2;
+ }
+
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
+
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- interface.stop ();
+ interface.start();
+
+ while (!viewer.wasStopped()) {
+ if (cloud_) {
+ // the call to get() sets the cloud_ to null;
+ viewer.showCloud(get());
+ }
}
- pcl::visualization::CloudViewer viewer;
- pcl::VoxelGrid<PointType> grid_;
- pcl::SACSegmentation<PointType> seg_;
- pcl::ExtractIndices<PointType> extract_;
+ interface.stop();
+ }
+
+ pcl::visualization::CloudViewer viewer;
+ pcl::VoxelGrid<PointType> grid_;
+ pcl::SACSegmentation<PointType> seg_;
+ pcl::ExtractIndices<PointType> extract_;
- std::string device_id_;
- std::mutex mtx_;
- CloudConstPtr cloud_;
+ std::string device_id_;
+ std::mutex mtx_;
+ CloudConstPtr cloud_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n"
- << "where options are:\n -thresh X :: set the planar segmentation threshold (default: 0.5)\n";
+ << "where options are:\n -thresh X :: set the planar "
+ "segmentation threshold (default: 0.5)\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
std::cout << "No devices connected." << std::endl;
}
-int
-main (int argc, char ** argv)
+int
+main(int argc, char** argv)
{
- if (argc < 2)
- {
- usage (argv);
+ if (argc < 2) {
+ usage(argv);
return 1;
}
- std::string arg (argv[1]);
-
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ std::string arg(argv[1]);
+
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
double threshold = 0.05;
- pcl::console::parse_argument (argc, argv, "-thresh", threshold);
+ pcl::console::parse_argument(argc, argv, "-thresh", threshold);
- pcl::OpenNIGrabber grabber (arg);
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- OpenNIPlanarSegmentation<pcl::PointXYZRGBA> v (arg, threshold);
- v.run ();
+ pcl::OpenNIGrabber grabber(arg);
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ OpenNIPlanarSegmentation<pcl::PointXYZRGBA> v(arg, threshold);
+ v.run();
}
- else
- {
- OpenNIPlanarSegmentation<pcl::PointXYZ> v (arg, threshold);
- v.run ();
+ else {
+ OpenNIPlanarSegmentation<pcl::PointXYZ> v(arg, threshold);
+ v.run();
}
- return (0);
+ return 0;
}
* Author: Julius Kammerl (julius@kammerl.de)
*/
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <boost/asio.hpp>
-#include <iostream>
-#include <vector>
#include <cstdio>
-#include <sstream>
#include <cstdlib>
#include <iostream>
+#include <sstream>
#include <string>
#include <thread>
-
-#include <boost/asio.hpp>
+#include <vector>
using boost::asio::ip::tcp;
using namespace std;
using namespace std::chrono_literals;
-class SimpleOpenNIViewer
-{
- public:
- SimpleOpenNIViewer () :
- viewer_ ("Input Point Cloud - Shift-to-depth conversion viewer"),
- grabber_(nullptr)
- {
- }
+class SimpleOpenNIViewer {
+public:
+ SimpleOpenNIViewer()
+ : viewer_("Input Point Cloud - Shift-to-depth conversion viewer")
+ , grabber_("",
+ pcl::OpenNIGrabber::OpenNI_Default_Mode,
+ pcl::OpenNIGrabber::OpenNI_Default_Mode)
+ {}
- ~SimpleOpenNIViewer ()
- {
- if (grabber_)
- delete grabber_;
- }
-
- void
- image_callback (const openni_wrapper::Image::Ptr &image,
- const openni_wrapper::DepthImage::Ptr &depth_image, float)
- {
-
- std::vector<std::uint16_t> raw_shift_data;
- std::vector<std::uint16_t> raw_depth_data;
-
- std::vector<std::uint8_t> rgb_data;
-
- std::uint32_t width=depth_image->getWidth ();
- std::uint32_t height=depth_image->getHeight ();
-
- // copy raw shift data from depth_image
- raw_shift_data.resize(width*height);
- depth_image->fillDepthImageRaw (width, height, &raw_shift_data[0], static_cast<unsigned int> (width * sizeof (std::uint16_t)));
+ void
+ image_callback(const openni_wrapper::Image::Ptr& image,
+ const openni_wrapper::DepthImage::Ptr& depth_image,
+ float)
+ {
- // convert raw shift data to raw depth data
- raw_depth_data.resize(width*height);
- grabber_->convertShiftToDepth(&raw_shift_data[0], &raw_depth_data[0], raw_shift_data.size());
+ std::vector<std::uint16_t> raw_shift_data;
+ std::vector<std::uint16_t> raw_depth_data;
+
+ std::vector<std::uint8_t> rgb_data;
+
+ std::uint32_t width = depth_image->getWidth();
+ std::uint32_t height = depth_image->getHeight();
+
+ // copy raw shift data from depth_image
+ raw_shift_data.resize(width * height);
+ depth_image->fillDepthImageRaw(
+ width,
+ height,
+ &raw_shift_data[0],
+ static_cast<unsigned int>(width * sizeof(std::uint16_t)));
+
+ // convert raw shift data to raw depth data
+ raw_depth_data.resize(width * height);
+ grabber_.convertShiftToDepth(
+ &raw_shift_data[0], &raw_depth_data[0], raw_shift_data.size());
+
+ // check for color data
+ if (image->getEncoding() != openni_wrapper::Image::RGB) {
+ // copy raw rgb data from image
+ rgb_data.resize(width * height * 3);
+ image->fillRGB(width,
+ height,
+ &rgb_data[0],
+ static_cast<unsigned int>(width * sizeof(std::uint8_t) * 3));
+ }
- // check for color data
- if (image->getEncoding() != openni_wrapper::Image::RGB)
- {
- // copy raw rgb data from image
- rgb_data.resize(width*height*3);
- image->fillRGB(width, height, &rgb_data[0], static_cast<unsigned int> (width * sizeof (std::uint8_t) * 3));
- }
+ // empty pointcloud
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud =
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>());
- // empty pointcloud
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>());
+ // convert raw depth and rgb data to pointcloud
+ convert(
+ raw_depth_data, rgb_data, width, height, depth_image->getFocalLength(), *cloud);
- // convert raw depth and rgb data to pointcloud
- convert (raw_depth_data,
- rgb_data,
- width,
- height,
- depth_image->getFocalLength(),
- *cloud);
+ // display pointcloud
+ viewer_.showCloud(cloud);
+ }
- // display pointcloud
- viewer_.showCloud (cloud);
+ void
+ run()
+ {
+ // initialize OpenNIDevice to shift-value mode
+ int depthformat = openni_wrapper::OpenNIDevice::OpenNI_shift_values;
+
+ // Set the depth output format
+ grabber_.getDevice()->setDepthOutputFormat(
+ static_cast<openni_wrapper::OpenNIDevice::DepthMode>(depthformat));
+
+ // define image callback
+ std::function<void(const openni_wrapper::Image::Ptr&,
+ const openni_wrapper::DepthImage::Ptr&,
+ float)>
+ image_cb = [this](const openni_wrapper::Image::Ptr& img,
+ const openni_wrapper::DepthImage::Ptr& depth,
+ float f) { image_callback(img, depth, f); };
+ grabber_.registerCallback(image_cb);
+
+ // start grabber thread
+ grabber_.start();
+ while (true) {
+ std::this_thread::sleep_for(1s);
}
-
- void
- run ()
- {
- // initialize OpenNIDevice to shift-value mode
- pcl::OpenNIGrabber::Mode image_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode;
- int depthformat = openni_wrapper::OpenNIDevice::OpenNI_shift_values;
-
- grabber_ = new pcl::OpenNIGrabber("", pcl::OpenNIGrabber::OpenNI_Default_Mode, image_mode);
-
- // Set the depth output format
- grabber_->getDevice ()->setDepthOutputFormat (static_cast<openni_wrapper::OpenNIDevice::DepthMode> (depthformat));
-
- // define image callback
- std::function<void
- (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float)> image_cb =
- [this] (const openni_wrapper::Image::Ptr& img, const openni_wrapper::DepthImage::Ptr& depth, float f)
- {
- image_callback (img, depth, f);
- };
- boost::signals2::connection image_connection = grabber_->registerCallback (image_cb);
-
- // start grabber thread
- grabber_->start ();
- while (true)
- {
- std::this_thread::sleep_for(1s);
- }
- grabber_->stop ();
-
+ grabber_.stop();
}
protected:
-
/* helper method to convert depth&rgb data to pointcloud*/
void
- convert (std::vector<std::uint16_t>& depthData_arg,
- std::vector<std::uint8_t>& rgbData_arg,
- std::size_t width_arg,
- std::size_t height_arg,
- float focalLength_arg,
-
pcl::PointCloud<PointXYZRGB>& cloud_arg) const
+ convert(std::vector<std::uint16_t>& depthData_arg,
+ std::vector<std::uint8_t>& rgbData_arg,
+ std::size_t width_arg,
+ std::size_t height_arg,
+ float focalLength_arg,
+ pcl::PointCloud<PointXYZRGB>& cloud_arg) const
{
std::size_t cloud_size = width_arg * height_arg;
// Reset point cloud
- cloud_arg.points.clear ();
- cloud_arg.points.reserve (cloud_size);
+ cloud_arg.points.clear();
+ cloud_arg.points.reserve(cloud_size);
// Define point cloud parameters
- cloud_arg.width = static_cast<std::uint32_t> (width_arg);
- cloud_arg.height = static_cast<std::uint32_t> (height_arg);
+ cloud_arg.width = static_cast<std::uint32_t>(width_arg);
+ cloud_arg.height = static_cast<std::uint32_t>(height_arg);
cloud_arg.is_dense = false;
// Calculate center of disparity image
- int centerX = static_cast<int> (width_arg / 2);
- int centerY = static_cast<int> (height_arg / 2);
+ int centerX = static_cast<int>(width_arg / 2);
+ int centerY = static_cast<int>(height_arg / 2);
const float fl_const = 1.0f / focalLength_arg;
- static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ static const float bad_point = std::numeric_limits<float>::quiet_NaN();
std::size_t i = 0;
for (int y = -centerY; y < +centerY; ++y)
- for (int x = -centerX; x < +centerX; ++x)
- {
+ for (int x = -centerX; x < +centerX; ++x) {
PointXYZRGB newPoint;
const std::uint16_t& pixel_depth = depthData_arg[i];
- if (pixel_depth)
- {
- float depth = pixel_depth/1000.0f; // raw mm -> m
+ if (pixel_depth) {
+ float depth = pixel_depth / 1000.0f; // raw mm -> m
// Define point location
newPoint.z = depth;
- newPoint.x = static_cast<float> (x) * depth * fl_const;
- newPoint.y = static_cast<float> (y) * depth * fl_const;
+ newPoint.x = static_cast<float>(x) * depth * fl_const;
+ newPoint.y = static_cast<float>(y) * depth * fl_const;
// Define point color
newPoint.r = rgbData_arg[i * 3 + 0];
newPoint.g = rgbData_arg[i * 3 + 1];
newPoint.b = rgbData_arg[i * 3 + 2];
}
- else
- {
+ else {
// Define bad point
newPoint.x = newPoint.y = newPoint.z = bad_point;
newPoint.rgba = 0;
}
// Add point to cloud
- cloud_arg.points.push_back (newPoint);
+ cloud_arg.points.push_back(newPoint);
// Increment point iterator
++i;
}
}
pcl::visualization::CloudViewer viewer_;
- pcl::OpenNIGrabber* grabber_;
-
+ pcl::OpenNIGrabber grabber_;
};
int
-main (int, char **)
+main(int, char**)
{
SimpleOpenNIViewer v;
- v.run ();
+ v.run();
- return (0);
+ return 0;
}
-
*
*/
-#include <pcl/tracking/tracking.h>
-#include <pcl/tracking/particle_filter.h>
-#include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
-#include <pcl/tracking/particle_filter_omp.h>
-#include <pcl/tracking/coherence.h>
-#include <pcl/tracking/distance_coherence.h>
-#include <pcl/tracking/hsv_color_coherence.h>
-#include <pcl/tracking/normal_coherence.h>
-#include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
-#include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
#include <pcl/common/centroid.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/io/pcd_io.h>
+#include <pcl/common/time.h>
+#include <pcl/common/transforms.h>
+#include <pcl/console/parse.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/normal_3d_omp.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/approximate_voxel_grid.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/features/normal_3d.h>
-#include <pcl/features/normal_3d_omp.h>
-#include <pcl/features/integral_image_normal.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
-#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/segmentation/extract_polygonal_prism_data.h>
+#include <pcl/search/pcl_search.h>
#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
+#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/surface/convex_hull.h>
-#include <pcl/search/pcl_search.h>
-#include <pcl/common/transforms.h>
+#include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
+#include <pcl/tracking/coherence.h>
+#include <pcl/tracking/distance_coherence.h>
+#include <pcl/tracking/hsv_color_coherence.h>
+#include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
+#include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
+#include <pcl/tracking/normal_coherence.h>
+#include <pcl/tracking/particle_filter.h>
+#include <pcl/tracking/particle_filter_omp.h>
+#include <pcl/tracking/tracking.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <boost/format.hpp>
#include <mutex>
#include <thread>
-#define FPS_CALC_BEGIN \
- static double duration = 0; \
- double start_time = pcl::getTime (); \
-
-#define FPS_CALC_END(_WHAT_) \
- { \
- double end_time = pcl::getTime (); \
- static unsigned count = 0; \
- if (++count == 10) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(duration) << " Hz" << std::endl; \
- count = 0; \
- duration = 0.0; \
- } \
- else \
- { \
- duration += end_time - start_time; \
- } \
+#define FPS_CALC_BEGIN \
+ static double duration = 0; \
+ double start_time = pcl::getTime();
+
+// clang-format off
+#define FPS_CALC_END(_WHAT_) \
+ { \
+ double end_time = pcl::getTime(); \
+ static unsigned count = 0; \
+ if (++count == 10) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(duration) << " Hz" << std::endl; \
+ count = 0; \
+ duration = 0.0; \
+ } \
+ else { \
+ duration += end_time - start_time; \
+ } \
}
+// clang-format on
using namespace pcl::tracking;
using namespace std::chrono_literals;
template <typename PointType>
-class OpenNISegmentTracking
-{
+class OpenNISegmentTracking {
public:
- //using RefPointType = pcl::PointXYZRGBANormal;
using RefPointType = pcl::PointXYZRGBA;
- //using RefPointType = pcl::PointXYZ;
using ParticleT = ParticleXYZRPY;
-
+
using Cloud = pcl::PointCloud<PointType>;
using RefCloud = pcl::PointCloud<RefPointType>;
using RefCloudPtr = RefCloud::Ptr;
using RefCloudConstPtr = RefCloud::ConstPtr;
using CloudPtr = typename Cloud::Ptr;
using CloudConstPtr = typename Cloud::ConstPtr;
- //using ParticleFilter = KLDAdaptiveParticleFilterTracker<RefPointType, ParticleT>;
- //using ParticleFilter = KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>;
- //using ParticleFilter = ParticleFilterOMPTracker<RefPointType, ParticleT>;
using ParticleFilter = ParticleFilterTracker<RefPointType, ParticleT>;
using CoherencePtr = ParticleFilter::CoherencePtr;
using KdTree = pcl::search::KdTree<PointType>;
using KdTreePtr = typename KdTree::Ptr;
- OpenNISegmentTracking (const std::string& device_id, int thread_nr, double downsampling_grid_size,
- bool use_convex_hull,
- bool visualize_non_downsample, bool visualize_particles,
- bool use_fixed)
- : viewer_ ("PCL OpenNI Tracking Viewer")
- , device_id_ (device_id)
- , new_cloud_ (false)
- , ne_ (thread_nr)
- , counter_ (0)
- , use_convex_hull_ (use_convex_hull)
- , visualize_non_downsample_ (visualize_non_downsample)
- , visualize_particles_ (visualize_particles)
- , downsampling_grid_size_ (downsampling_grid_size)
+ OpenNISegmentTracking(const std::string& device_id,
+ int thread_nr,
+ double downsampling_grid_size,
+ bool use_convex_hull,
+ bool visualize_non_downsample,
+ bool visualize_particles,
+ bool use_fixed)
+ : viewer_("PCL OpenNI Tracking Viewer")
+ , device_id_(device_id)
+ , new_cloud_(false)
+ , ne_(thread_nr)
+ , counter_(0)
+ , use_convex_hull_(use_convex_hull)
+ , visualize_non_downsample_(visualize_non_downsample)
+ , visualize_particles_(visualize_particles)
+ , downsampling_grid_size_(downsampling_grid_size)
{
- KdTreePtr tree (new KdTree (false));
- ne_.setSearchMethod (tree);
- ne_.setRadiusSearch (0.03);
-
- std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
+ KdTreePtr tree(new KdTree(false));
+ ne_.setSearchMethod(tree);
+ ne_.setRadiusSearch(0.03);
+
+ std::vector<double> default_step_covariance = std::vector<double>(6, 0.015 * 0.015);
default_step_covariance[3] *= 40.0;
default_step_covariance[4] *= 40.0;
default_step_covariance[5] *= 40.0;
-
- std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
- std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);
- if (use_fixed)
- {
- ParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker
- (new ParticleFilterOMPTracker<RefPointType, ParticleT> (thread_nr));
+
+ std::vector<double> initial_noise_covariance = std::vector<double>(6, 0.00001);
+ std::vector<double> default_initial_mean = std::vector<double>(6, 0.0);
+ if (use_fixed) {
+ ParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker(
+ new ParticleFilterOMPTracker<RefPointType, ParticleT>(thread_nr));
tracker_ = tracker;
}
- else
- {
- KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker
- (new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (thread_nr));
- tracker->setMaximumParticleNum (500);
- tracker->setDelta (0.99);
- tracker->setEpsilon (0.2);
+ else {
+ KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker(
+ new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>(thread_nr));
+ tracker->setMaximumParticleNum(500);
+ tracker->setDelta(0.99);
+ tracker->setEpsilon(0.2);
ParticleT bin_size;
bin_size.x = 0.1f;
bin_size.y = 0.1f;
bin_size.roll = 0.1f;
bin_size.pitch = 0.1f;
bin_size.yaw = 0.1f;
- tracker->setBinSize (bin_size);
+ tracker->setBinSize(bin_size);
tracker_ = tracker;
}
-
- tracker_->setTrans (Eigen::Affine3f::Identity ());
- tracker_->setStepNoiseCovariance (default_step_covariance);
- tracker_->setInitialNoiseCovariance (initial_noise_covariance);
- tracker_->setInitialNoiseMean (default_initial_mean);
- tracker_->setIterationNum (1);
-
- tracker_->setParticleNum (400);
+
+ tracker_->setTrans(Eigen::Affine3f::Identity());
+ tracker_->setStepNoiseCovariance(default_step_covariance);
+ tracker_->setInitialNoiseCovariance(initial_noise_covariance);
+ tracker_->setInitialNoiseMean(default_initial_mean);
+ tracker_->setIterationNum(1);
+
+ tracker_->setParticleNum(400);
tracker_->setResampleLikelihoodThr(0.00);
- tracker_->setUseNormal (false);
+ tracker_->setUseNormal(false);
// setup coherences
- ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence = ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr
- (new ApproxNearestPairPointCloudCoherence<RefPointType> ());
- // NearestPairPointCloudCoherence<RefPointType>::Ptr coherence = NearestPairPointCloudCoherence<RefPointType>::Ptr
- // (new NearestPairPointCloudCoherence<RefPointType> ());
-
- DistanceCoherence<RefPointType>::Ptr distance_coherence (new DistanceCoherence<RefPointType>);
- coherence->addPointCoherence (distance_coherence);
-
- HSVColorCoherence<RefPointType>::Ptr color_coherence (new HSVColorCoherence<RefPointType>);
- color_coherence->setWeight (0.1);
- coherence->addPointCoherence (color_coherence);
-
- pcl::search::Octree<RefPointType>::Ptr search (new pcl::search::Octree<RefPointType> (0.01));
- coherence->setSearchMethod (search);
- coherence->setMaximumDistance (0.01);
- tracker_->setCloudCoherence (coherence);
+ ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence =
+ ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr(
+ new ApproxNearestPairPointCloudCoherence<RefPointType>());
+
+ DistanceCoherence<RefPointType>::Ptr distance_coherence(
+ new DistanceCoherence<RefPointType>);
+ coherence->addPointCoherence(distance_coherence);
+
+ HSVColorCoherence<RefPointType>::Ptr color_coherence(
+ new HSVColorCoherence<RefPointType>);
+ color_coherence->setWeight(0.1);
+ coherence->addPointCoherence(color_coherence);
+
+ pcl::search::Octree<RefPointType>::Ptr search(
+ new pcl::search::Octree<RefPointType>(0.01));
+ coherence->setSearchMethod(search);
+ coherence->setMaximumDistance(0.01);
+ tracker_->setCloudCoherence(coherence);
}
bool
- drawParticles (pcl::visualization::PCLVisualizer& viz)
+ drawParticles(pcl::visualization::PCLVisualizer& viz)
{
- ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
- if (particles)
- {
- if (visualize_particles_)
- {
- pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
- for (const auto &point : particles->points)
- {
- particle_cloud->points.emplace_back (point.x, point.y, point.z);
+ ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles();
+ if (particles) {
+ if (visualize_particles_) {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud(
+ new pcl::PointCloud<pcl::PointXYZ>());
+ for (const auto& point : particles->points) {
+ particle_cloud->points.emplace_back(point.x, point.y, point.z);
}
-
+
{
- pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color (particle_cloud, 250, 99, 71);
- if (!viz.updatePointCloud (particle_cloud, blue_color, "particle cloud"))
- viz.addPointCloud (particle_cloud, blue_color, "particle cloud");
+ pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color(
+ particle_cloud, 250, 99, 71);
+ if (!viz.updatePointCloud(particle_cloud, blue_color, "particle cloud"))
+ viz.addPointCloud(particle_cloud, blue_color, "particle cloud");
}
}
return true;
}
- PCL_WARN ("no particles\n");
+ PCL_WARN("no particles\n");
return false;
}
-
+
void
- drawResult (pcl::visualization::PCLVisualizer& viz)
+ drawResult(pcl::visualization::PCLVisualizer& viz)
{
- ParticleXYZRPY result = tracker_->getResult ();
- Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
+ ParticleXYZRPY result = tracker_->getResult();
+ Eigen::Affine3f transformation = tracker_->toEigenMatrix(result);
// move a little bit for better visualization
- transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
- RefCloudPtr result_cloud (new RefCloud ());
+ transformation.translation() += Eigen::Vector3f(0.0f, 0.0f, -0.005f);
+ RefCloudPtr result_cloud(new RefCloud());
if (!visualize_non_downsample_)
- pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
+ pcl::transformPointCloud<RefPointType>(
+ *(tracker_->getReferenceCloud()), *result_cloud, transformation);
else
- pcl::transformPointCloud<RefPointType> (*reference_, *result_cloud, transformation);
+ pcl::transformPointCloud<RefPointType>(
+ *reference_, *result_cloud, transformation);
{
- pcl::visualization::PointCloudColorHandlerCustom<RefPointType> red_color (result_cloud, 0, 0, 255);
- if (!viz.updatePointCloud (result_cloud, red_color, "resultcloud"))
- viz.addPointCloud (result_cloud, red_color, "resultcloud");
+ pcl::visualization::PointCloudColorHandlerCustom<RefPointType> red_color(
+ result_cloud, 0, 0, 255);
+ if (!viz.updatePointCloud(result_cloud, red_color, "resultcloud"))
+ viz.addPointCloud(result_cloud, red_color, "resultcloud");
}
-
}
-
+
void
- viz_cb (pcl::visualization::PCLVisualizer& viz)
+ viz_cb(pcl::visualization::PCLVisualizer& viz)
{
- std::lock_guard<std::mutex> lock (mtx_);
-
- if (!cloud_pass_)
- {
+ std::lock_guard<std::mutex> lock(mtx_);
+
+ if (!cloud_pass_) {
std::this_thread::sleep_for(1s);
return;
}
-
- if (new_cloud_ && cloud_pass_downsampled_)
- {
+
+ if (new_cloud_ && cloud_pass_downsampled_) {
CloudPtr cloud_pass;
if (!visualize_non_downsample_)
cloud_pass = cloud_pass_downsampled_;
else
cloud_pass = cloud_pass_;
-
- if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
- {
- viz.addPointCloud (cloud_pass, "cloudpass");
- viz.resetCameraViewpoint ("cloudpass");
- }
+
+ if (!viz.updatePointCloud(cloud_pass, "cloudpass")) {
+ viz.addPointCloud(cloud_pass, "cloudpass");
+ viz.resetCameraViewpoint("cloudpass");
+ }
}
- if (new_cloud_ && reference_)
- {
- bool ret = drawParticles (viz);
- if (ret)
- {
- drawResult (viz);
-
+ if (new_cloud_ && reference_) {
+ bool ret = drawParticles(viz);
+ if (ret) {
+ drawResult(viz);
+
+ // clang-format off
// draw some texts
- viz.removeShape ("N");
- viz.addText ((boost::format ("number of Reference PointClouds: %d") % tracker_->getReferenceCloud ()->points.size ()).str (),
- 10, 20, 20, 1.0, 1.0, 1.0, "N");
-
- viz.removeShape ("M");
- viz.addText ((boost::format ("number of Measured PointClouds: %d") % cloud_pass_downsampled_->points.size ()).str (),
- 10, 40, 20, 1.0, 1.0, 1.0, "M");
-
- viz.removeShape ("tracking");
- viz.addText ((boost::format ("tracking: %f fps") % (1.0 / tracking_time_)).str (),
- 10, 60, 20, 1.0, 1.0, 1.0, "tracking");
-
- viz.removeShape ("downsampling");
- viz.addText ((boost::format ("downsampling: %f fps") % (1.0 / downsampling_time_)).str (),
- 10, 80, 20, 1.0, 1.0, 1.0, "downsampling");
-
- viz.removeShape ("computation");
- viz.addText ((boost::format ("computation: %f fps") % (1.0 / computation_time_)).str (),
- 10, 100, 20, 1.0, 1.0, 1.0, "computation");
-
- viz.removeShape ("particles");
- viz.addText ((boost::format ("particles: %d") % tracker_->getParticles ()->points.size ()).str (),
- 10, 120, 20, 1.0, 1.0, 1.0, "particles");
-
+ viz.removeShape("N");
+ viz.addText((boost::format("number of Reference PointClouds: %d") %
+ tracker_->getReferenceCloud()->points.size()).str(),
+ 10, 20, 20, 1.0, 1.0, 1.0, "N");
+
+ viz.removeShape("M");
+ viz.addText((boost::format("number of Measured PointClouds: %d") %
+ cloud_pass_downsampled_->points.size()).str(),
+ 10, 40, 20, 1.0, 1.0, 1.0, "M");
+
+ viz.removeShape("tracking");
+ viz.addText((boost::format("tracking: %f fps") % (1.0 / tracking_time_)).str(),
+ 10, 60, 20, 1.0, 1.0, 1.0, "tracking");
+
+ viz.removeShape("downsampling");
+ viz.addText((boost::format("downsampling: %f fps") % (1.0 / downsampling_time_)).str(),
+ 10, 80, 20, 1.0, 1.0, 1.0, "downsampling");
+
+ viz.removeShape("computation");
+ viz.addText((boost::format("computation: %f fps") % (1.0 / computation_time_)).str(),
+ 10, 100, 20, 1.0, 1.0, 1.0, "computation");
+
+ viz.removeShape("particles");
+ viz.addText((boost::format("particles: %d") %
+ tracker_->getParticles()->points.size()).str(),
+ 10, 120, 20, 1.0, 1.0, 1.0, "particles");
+ // clang-format on
}
}
new_cloud_ = false;
}
- void filterPassThrough (const CloudConstPtr &cloud, Cloud &result)
+ void
+ filterPassThrough(const CloudConstPtr& cloud, Cloud& result)
{
FPS_CALC_BEGIN;
pcl::PassThrough<PointType> pass;
- pass.setFilterFieldName ("z");
- pass.setFilterLimits (0.0, 10.0);
- //pass.setFilterLimits (0.0, 1.5);
- //pass.setFilterLimits (0.0, 0.6);
- pass.setKeepOrganized (false);
- pass.setInputCloud (cloud);
- pass.filter (result);
+ pass.setFilterFieldName("z");
+ pass.setFilterLimits(0.0, 10.0);
+ pass.setKeepOrganized(false);
+ pass.setInputCloud(cloud);
+ pass.filter(result);
FPS_CALC_END("filterPassThrough");
}
- void euclideanSegment (const CloudConstPtr &cloud,
- std::vector<pcl::PointIndices> &cluster_indices)
+ void
+ euclideanSegment(const CloudConstPtr& cloud,
+ std::vector<pcl::PointIndices>& cluster_indices)
{
FPS_CALC_BEGIN;
pcl::EuclideanClusterExtraction<PointType> ec;
- KdTreePtr tree (new KdTree ());
-
- ec.setClusterTolerance (0.05); // 2cm
- ec.setMinClusterSize (50);
- ec.setMaxClusterSize (25000);
- //ec.setMaxClusterSize (400);
- ec.setSearchMethod (tree);
- ec.setInputCloud (cloud);
- ec.extract (cluster_indices);
+ KdTreePtr tree(new KdTree());
+
+ ec.setClusterTolerance(0.05); // 2cm
+ ec.setMinClusterSize(50);
+ ec.setMaxClusterSize(25000);
+ ec.setSearchMethod(tree);
+ ec.setInputCloud(cloud);
+ ec.extract(cluster_indices);
FPS_CALC_END("euclideanSegmentation");
}
-
- void gridSample (const CloudConstPtr &cloud, Cloud &result, double leaf_size = 0.01)
+
+ void
+ gridSample(const CloudConstPtr& cloud, Cloud& result, double leaf_size = 0.01)
{
FPS_CALC_BEGIN;
- double start = pcl::getTime ();
+ double start = pcl::getTime();
pcl::VoxelGrid<PointType> grid;
- //pcl::ApproximateVoxelGrid<PointType> grid;
- grid.setLeafSize (float (leaf_size), float (leaf_size), float (leaf_size));
- grid.setInputCloud (cloud);
- grid.filter (result);
- //result = *cloud;
- double end = pcl::getTime ();
+ grid.setLeafSize(float(leaf_size), float(leaf_size), float(leaf_size));
+ grid.setInputCloud(cloud);
+ grid.filter(result);
+ double end = pcl::getTime();
downsampling_time_ = end - start;
FPS_CALC_END("gridSample");
}
-
- void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size = 0.01)
+
+ void
+ gridSampleApprox(const CloudConstPtr& cloud, Cloud& result, double leaf_size = 0.01)
{
FPS_CALC_BEGIN;
- double start = pcl::getTime ();
- //pcl::VoxelGrid<PointType> grid;
+ double start = pcl::getTime();
pcl::ApproximateVoxelGrid<PointType> grid;
- grid.setLeafSize (static_cast<float> (leaf_size), static_cast<float> (leaf_size), static_cast<float> (leaf_size));
- grid.setInputCloud (cloud);
- grid.filter (result);
- //result = *cloud;
- double end = pcl::getTime ();
+ grid.setLeafSize(static_cast<float>(leaf_size),
+ static_cast<float>(leaf_size),
+ static_cast<float>(leaf_size));
+ grid.setInputCloud(cloud);
+ grid.filter(result);
+ double end = pcl::getTime();
downsampling_time_ = end - start;
FPS_CALC_END("gridSample");
}
-
- void planeSegmentation (const CloudConstPtr &cloud,
- pcl::ModelCoefficients &coefficients,
- pcl::PointIndices &inliers)
+
+ void
+ planeSegmentation(const CloudConstPtr& cloud,
+ pcl::ModelCoefficients& coefficients,
+ pcl::PointIndices& inliers)
{
FPS_CALC_BEGIN;
pcl::SACSegmentation<PointType> seg;
- seg.setOptimizeCoefficients (true);
- seg.setModelType (pcl::SACMODEL_PLANE);
- seg.setMethodType (pcl::SAC_RANSAC);
- seg.setMaxIterations (1000);
- seg.setDistanceThreshold (0.03);
- seg.setInputCloud (cloud);
- seg.segment (inliers, coefficients);
+ seg.setOptimizeCoefficients(true);
+ seg.setModelType(pcl::SACMODEL_PLANE);
+ seg.setMethodType(pcl::SAC_RANSAC);
+ seg.setMaxIterations(1000);
+ seg.setDistanceThreshold(0.03);
+ seg.setInputCloud(cloud);
+ seg.segment(inliers, coefficients);
FPS_CALC_END("planeSegmentation");
}
- void planeProjection (const CloudConstPtr &cloud,
- Cloud &result,
- const pcl::ModelCoefficients::ConstPtr &coefficients)
+ void
+ planeProjection(const CloudConstPtr& cloud,
+ Cloud& result,
+ const pcl::ModelCoefficients::ConstPtr& coefficients)
{
FPS_CALC_BEGIN;
pcl::ProjectInliers<PointType> proj;
- proj.setModelType (pcl::SACMODEL_PLANE);
- proj.setInputCloud (cloud);
- proj.setModelCoefficients (coefficients);
- proj.filter (result);
+ proj.setModelType(pcl::SACMODEL_PLANE);
+ proj.setInputCloud(cloud);
+ proj.setModelCoefficients(coefficients);
+ proj.filter(result);
FPS_CALC_END("planeProjection");
}
- void convexHull (const CloudConstPtr &cloud,
- Cloud &,
- std::vector<pcl::Vertices> &hull_vertices)
+ void
+ convexHull(const CloudConstPtr& cloud,
+ Cloud&,
+ std::vector<pcl::Vertices>& hull_vertices)
{
FPS_CALC_BEGIN;
pcl::ConvexHull<PointType> chull;
- chull.setInputCloud (cloud);
- chull.reconstruct (*cloud_hull_, hull_vertices);
+ chull.setInputCloud(cloud);
+ chull.reconstruct(*cloud_hull_, hull_vertices);
FPS_CALC_END("convexHull");
}
- void normalEstimation (const CloudConstPtr &cloud,
- pcl::PointCloud<pcl::Normal> &result)
+ void
+ normalEstimation(const CloudConstPtr& cloud, pcl::PointCloud<pcl::Normal>& result)
{
FPS_CALC_BEGIN;
- ne_.setInputCloud (cloud);
- ne_.compute (result);
+ ne_.setInputCloud(cloud);
+ ne_.compute(result);
FPS_CALC_END("normalEstimation");
}
-
- void tracking (const RefCloudConstPtr &cloud)
+
+ void
+ tracking(const RefCloudConstPtr& cloud)
{
- double start = pcl::getTime ();
+ double start = pcl::getTime();
FPS_CALC_BEGIN;
- tracker_->setInputCloud (cloud);
- tracker_->compute ();
- double end = pcl::getTime ();
+ tracker_->setInputCloud(cloud);
+ tracker_->compute();
+ double end = pcl::getTime();
FPS_CALC_END("tracking");
tracking_time_ = end - start;
}
- void addNormalToCloud (const CloudConstPtr &cloud,
- const pcl::PointCloud<pcl::Normal>::ConstPtr &,
- RefCloud &result)
+ void
+ addNormalToCloud(const CloudConstPtr& cloud,
+ const pcl::PointCloud<pcl::Normal>::ConstPtr&,
+ RefCloud& result)
{
result.width = cloud->width;
result.height = cloud->height;
result.is_dense = cloud->is_dense;
- for (std::size_t i = 0; i < cloud->points.size (); i++)
- {
+ for (std::size_t i = 0; i < cloud->points.size(); i++) {
RefPointType point;
point.x = cloud->points[i].x;
point.y = cloud->points[i].y;
point.z = cloud->points[i].z;
point.rgba = cloud->points[i].rgba;
- // point.normal[0] = normals->points[i].normal[0];
- // point.normal[1] = normals->points[i].normal[1];
- // point.normal[2] = normals->points[i].normal[2];
- result.points.push_back (point);
+ result.points.push_back(point);
}
}
- void extractNonPlanePoints (const CloudConstPtr &cloud,
- const CloudConstPtr &cloud_hull,
- Cloud &result)
+ void
+ extractNonPlanePoints(const CloudConstPtr& cloud,
+ const CloudConstPtr& cloud_hull,
+ Cloud& result)
{
pcl::ExtractPolygonalPrismData<PointType> polygon_extract;
- pcl::PointIndices::Ptr inliers_polygon (new pcl::PointIndices ());
- polygon_extract.setHeightLimits (0.01, 10.0);
- polygon_extract.setInputPlanarHull (cloud_hull);
- polygon_extract.setInputCloud (cloud);
- polygon_extract.segment (*inliers_polygon);
+ pcl::PointIndices::Ptr inliers_polygon(new pcl::PointIndices());
+ polygon_extract.setHeightLimits(0.01, 10.0);
+ polygon_extract.setInputPlanarHull(cloud_hull);
+ polygon_extract.setInputCloud(cloud);
+ polygon_extract.segment(*inliers_polygon);
{
pcl::ExtractIndices<PointType> extract_positive;
- extract_positive.setNegative (false);
- extract_positive.setInputCloud (cloud);
- extract_positive.setIndices (inliers_polygon);
- extract_positive.filter (result);
+ extract_positive.setNegative(false);
+ extract_positive.setInputCloud(cloud);
+ extract_positive.setIndices(inliers_polygon);
+ extract_positive.filter(result);
}
}
- void removeZeroPoints (const CloudConstPtr &cloud,
- Cloud &result)
+ void
+ removeZeroPoints(const CloudConstPtr& cloud, Cloud& result)
{
- for (std::size_t i = 0; i < cloud->points.size (); i++)
- {
+ for (std::size_t i = 0; i < cloud->points.size(); i++) {
PointType point = cloud->points[i];
- if (!(std::abs(point.x) < 0.01 &&
- std::abs(point.y) < 0.01 &&
+ if (!(std::abs(point.x) < 0.01 && std::abs(point.y) < 0.01 &&
std::abs(point.z) < 0.01) &&
- !std::isnan(point.x) &&
- !std::isnan(point.y) &&
- !std::isnan(point.z))
+ !std::isnan(point.x) && !std::isnan(point.y) && !std::isnan(point.z))
result.points.push_back(point);
}
- result.width = static_cast<std::uint32_t> (result.points.size ());
+ result.width = static_cast<std::uint32_t>(result.points.size());
result.height = 1;
result.is_dense = true;
}
-
- void extractSegmentCluster (const CloudConstPtr &cloud,
- const std::vector<pcl::PointIndices> &cluster_indices,
- const int segment_index,
- Cloud &result)
+
+ void
+ extractSegmentCluster(const CloudConstPtr& cloud,
+ const std::vector<pcl::PointIndices>& cluster_indices,
+ const int segment_index,
+ Cloud& result)
{
pcl::PointIndices segmented_indices = cluster_indices[segment_index];
- for (const int &index : segmented_indices.indices)
- {
+ for (const int& index : segmented_indices.indices) {
PointType point = cloud->points[index];
- result.points.push_back (point);
+ result.points.push_back(point);
}
- result.width = std::uint32_t (result.points.size ());
+ result.width = std::uint32_t(result.points.size());
result.height = 1;
result.is_dense = true;
}
-
+
void
- cloud_cb (const CloudConstPtr &cloud)
+ cloud_cb(const CloudConstPtr& cloud)
{
- std::lock_guard<std::mutex> lock (mtx_);
- double start = pcl::getTime ();
+ std::lock_guard<std::mutex> lock(mtx_);
+ double start = pcl::getTime();
FPS_CALC_BEGIN;
- cloud_pass_.reset (new Cloud);
- cloud_pass_downsampled_.reset (new Cloud);
- pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
- pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
- filterPassThrough (cloud, *cloud_pass_);
- if (counter_ < 10)
- {
- gridSample (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
+ cloud_pass_.reset(new Cloud);
+ cloud_pass_downsampled_.reset(new Cloud);
+ pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
+ filterPassThrough(cloud, *cloud_pass_);
+ if (counter_ < 10) {
+ gridSample(cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
}
- else if (counter_ == 10)
- {
- //gridSample (cloud_pass_, *cloud_pass_downsampled_, 0.01);
+ else if (counter_ == 10) {
cloud_pass_downsampled_ = cloud_pass_;
CloudPtr target_cloud;
- if (use_convex_hull_)
- {
- planeSegmentation (cloud_pass_downsampled_, *coefficients, *inliers);
- if (inliers->indices.size () > 3)
- {
- CloudPtr cloud_projected (new Cloud);
- cloud_hull_.reset (new Cloud);
- nonplane_cloud_.reset (new Cloud);
-
- planeProjection (cloud_pass_downsampled_, *cloud_projected, coefficients);
- convexHull (cloud_projected, *cloud_hull_, hull_vertices_);
-
- extractNonPlanePoints (cloud_pass_downsampled_, cloud_hull_, *nonplane_cloud_);
+ if (use_convex_hull_) {
+ planeSegmentation(cloud_pass_downsampled_, *coefficients, *inliers);
+ if (inliers->indices.size() > 3) {
+ CloudPtr cloud_projected(new Cloud);
+ cloud_hull_.reset(new Cloud);
+ nonplane_cloud_.reset(new Cloud);
+
+ planeProjection(cloud_pass_downsampled_, *cloud_projected, coefficients);
+ convexHull(cloud_projected, *cloud_hull_, hull_vertices_);
+
+ extractNonPlanePoints(cloud_pass_downsampled_, cloud_hull_, *nonplane_cloud_);
target_cloud = nonplane_cloud_;
}
- else
- {
- PCL_WARN ("cannot segment plane\n");
+ else {
+ PCL_WARN("cannot segment plane\n");
}
}
- else
- {
- PCL_WARN ("without plane segmentation\n");
+ else {
+ PCL_WARN("without plane segmentation\n");
target_cloud = cloud_pass_downsampled_;
}
-
- if (target_cloud != nullptr)
- {
- PCL_INFO ("segmentation, please wait...\n");
+
+ if (target_cloud != nullptr) {
+ PCL_INFO("segmentation, please wait...\n");
std::vector<pcl::PointIndices> cluster_indices;
- euclideanSegment (target_cloud, cluster_indices);
- if (!cluster_indices.empty ())
- {
+ euclideanSegment(target_cloud, cluster_indices);
+ if (!cluster_indices.empty()) {
// select the cluster to track
- CloudPtr temp_cloud (new Cloud);
- extractSegmentCluster (target_cloud, cluster_indices, 0, *temp_cloud);
+ CloudPtr temp_cloud(new Cloud);
+ extractSegmentCluster(target_cloud, cluster_indices, 0, *temp_cloud);
Eigen::Vector4f c;
- pcl::compute3DCentroid<RefPointType> (*temp_cloud, c);
+ pcl::compute3DCentroid<RefPointType>(*temp_cloud, c);
int segment_index = 0;
double segment_distance = c[0] * c[0] + c[1] * c[1];
- for (std::size_t i = 1; i < cluster_indices.size (); i++)
- {
- temp_cloud.reset (new Cloud);
- extractSegmentCluster (target_cloud, cluster_indices, int (i), *temp_cloud);
- pcl::compute3DCentroid<RefPointType> (*temp_cloud, c);
+ for (std::size_t i = 1; i < cluster_indices.size(); i++) {
+ temp_cloud.reset(new Cloud);
+ extractSegmentCluster(target_cloud, cluster_indices, int(i), *temp_cloud);
+ pcl::compute3DCentroid<RefPointType>(*temp_cloud, c);
double distance = c[0] * c[0] + c[1] * c[1];
- if (distance < segment_distance)
- {
- segment_index = int (i);
+ if (distance < segment_distance) {
+ segment_index = int(i);
segment_distance = distance;
}
}
-
- segmented_cloud_.reset (new Cloud);
- extractSegmentCluster (target_cloud, cluster_indices, segment_index, *segmented_cloud_);
- //pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
- //normalEstimation (segmented_cloud_, *normals);
- RefCloudPtr ref_cloud (new RefCloud);
- //addNormalToCloud (segmented_cloud_, normals, *ref_cloud);
+
+ segmented_cloud_.reset(new Cloud);
+ extractSegmentCluster(
+ target_cloud, cluster_indices, segment_index, *segmented_cloud_);
+ RefCloudPtr ref_cloud(new RefCloud);
ref_cloud = segmented_cloud_;
- RefCloudPtr nonzero_ref (new RefCloud);
- removeZeroPoints (ref_cloud, *nonzero_ref);
-
- PCL_INFO ("calculating cog\n");
-
- RefCloudPtr transed_ref (new RefCloud);
- pcl::compute3DCentroid<RefPointType> (*nonzero_ref, c);
- Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
- trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
- //pcl::transformPointCloudWithNormals<RefPointType> (*ref_cloud, *transed_ref, trans.inverse());
- pcl::transformPointCloud<RefPointType> (*nonzero_ref, *transed_ref, trans.inverse());
- CloudPtr transed_ref_downsampled (new Cloud);
- gridSample (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
- tracker_->setReferenceCloud (transed_ref_downsampled);
- tracker_->setTrans (trans);
+ RefCloudPtr nonzero_ref(new RefCloud);
+ removeZeroPoints(ref_cloud, *nonzero_ref);
+
+ PCL_INFO("calculating cog\n");
+
+ RefCloudPtr transed_ref(new RefCloud);
+ pcl::compute3DCentroid<RefPointType>(*nonzero_ref, c);
+ Eigen::Affine3f trans = Eigen::Affine3f::Identity();
+ trans.translation().matrix() = Eigen::Vector3f(c[0], c[1], c[2]);
+ pcl::transformPointCloud<RefPointType>(
+ *nonzero_ref, *transed_ref, trans.inverse());
+ CloudPtr transed_ref_downsampled(new Cloud);
+ gridSample(transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
+ tracker_->setReferenceCloud(transed_ref_downsampled);
+ tracker_->setTrans(trans);
reference_ = transed_ref;
- tracker_->setMinIndices (int (ref_cloud->points.size ()) / 2);
+ tracker_->setMinIndices(int(ref_cloud->points.size()) / 2);
}
- else
- {
- PCL_WARN ("euclidean segmentation failed\n");
+ else {
+ PCL_WARN("euclidean segmentation failed\n");
}
}
}
- else
- {
- //normals_.reset (new pcl::PointCloud<pcl::Normal>);
- //normalEstimation (cloud_pass_downsampled_, *normals_);
- //RefCloudPtr tracking_cloud (new RefCloud ());
- //addNormalToCloud (cloud_pass_downsampled_, normals_, *tracking_cloud);
- //tracking_cloud = cloud_pass_downsampled_;
-
- //*cloud_pass_downsampled_ = *cloud_pass_;
- //cloud_pass_downsampled_ = cloud_pass_;
- gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
- tracking (cloud_pass_downsampled_);
+ else {
+ gridSampleApprox(cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
+ tracking(cloud_pass_downsampled_);
}
-
+
new_cloud_ = true;
- double end = pcl::getTime ();
+ double end = pcl::getTime();
computation_time_ = end - start;
FPS_CALC_END("computation");
counter_++;
}
-
+
void
- run ()
+ run()
{
- pcl::OpenNIGrabber interface {device_id_};
- std::function<void (const CloudConstPtr&)> f =
- [this] (const CloudConstPtr& cloud) { cloud_cb (cloud); };
- interface.registerCallback (f);
-
- viewer_.runOnVisualizationThread ([this] (pcl::visualization::PCLVisualizer& viz) { viz_cb (viz); }, "viz_cb");
-
- interface.start ();
-
- while (!viewer_.wasStopped ())
+ pcl::OpenNIGrabber interface{device_id_};
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
+ interface.registerCallback(f);
+
+ viewer_.runOnVisualizationThread(
+ [this](pcl::visualization::PCLVisualizer& viz) { viz_cb(viz); }, "viz_cb");
+
+ interface.start();
+
+ while (!viewer_.wasStopped())
std::this_thread::sleep_for(1s);
- interface.stop ();
+ interface.stop();
}
-
+
pcl::visualization::CloudViewer viewer_;
pcl::PointCloud<pcl::Normal>::Ptr normals_;
CloudPtr cloud_pass_;
CloudPtr segmented_cloud_;
CloudPtr reference_;
std::vector<pcl::Vertices> hull_vertices_;
-
+
std::string device_id_;
std::mutex mtx_;
bool new_cloud_;
double computation_time_;
double downsampling_time_;
double downsampling_grid_size_;
- };
+};
void
-usage (char** argv)
+usage(char** argv)
{
+ // clang-format off
std::cout << "usage: " << argv[0] << " <device_id> [-C] [-g]\n\n";
- std::cout << " -C: initialize the pointcloud to track without plane segmentation"
- << std::endl;
- std::cout << " -D: visualizing with non-downsampled pointclouds."
- << std::endl;
- std::cout << " -P: not visualizing particle cloud."
- << std::endl;
- std::cout << " -fixed: use the fixed number of the particles."
- << std::endl;
- std::cout << " -d <value>: specify the grid size of downsampling (defaults to 0.01)."
- << std::endl;
+ std::cout << " -C: initialize the pointcloud to track without plane segmentation\n";
+ std::cout << " -D: visualizing with non-downsampled pointclouds.\n";
+ std::cout << " -P: not visualizing particle cloud.\n";
+ std::cout << " -fixed: use the fixed number of the particles.\n";
+ std::cout << " -d <value>: specify the grid size of downsampling (defaults to 0.01)." << std::endl;
+ // clang-format on
}
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
bool use_convex_hull = true;
bool visualize_non_downsample = false;
bool use_fixed = false;
double downsampling_grid_size = 0.01;
-
- if (pcl::console::find_argument (argc, argv, "-C") > 0)
+
+ if (pcl::console::find_argument(argc, argv, "-C") > 0)
use_convex_hull = false;
- if (pcl::console::find_argument (argc, argv, "-D") > 0)
+ if (pcl::console::find_argument(argc, argv, "-D") > 0)
visualize_non_downsample = true;
- if (pcl::console::find_argument (argc, argv, "-P") > 0)
+ if (pcl::console::find_argument(argc, argv, "-P") > 0)
visualize_particles = false;
- if (pcl::console::find_argument (argc, argv, "-fixed") > 0)
+ if (pcl::console::find_argument(argc, argv, "-fixed") > 0)
use_fixed = true;
- pcl::console::parse_argument (argc, argv, "-d", downsampling_grid_size);
- if (argc < 2)
- {
- usage (argv);
- exit (1);
+ pcl::console::parse_argument(argc, argv, "-d", downsampling_grid_size);
+ if (argc < 2) {
+ usage(argv);
+ exit(1);
}
-
- std::string device_id = std::string (argv[1]);
- if (device_id == "--help" || device_id == "-h")
- {
- usage (argv);
- exit (1);
+ std::string device_id = std::string(argv[1]);
+
+ if (device_id == "--help" || device_id == "-h") {
+ usage(argv);
+ exit(1);
}
-
+
// open kinect
- OpenNISegmentTracking<pcl::PointXYZRGBA> v (device_id, 8, downsampling_grid_size,
- use_convex_hull,
- visualize_non_downsample, visualize_particles,
- use_fixed);
- v.run ();
+ OpenNISegmentTracking<pcl::PointXYZRGBA> v(device_id,
+ 8,
+ downsampling_grid_size,
+ use_convex_hull,
+ visualize_non_downsample,
+ visualize_particles,
+ use_fixed);
+ v.run();
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/filters/uniform_sampling.h>
+#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/filters/uniform_sampling.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
#include <thread>
using namespace std::chrono_literals;
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
-
-
-class OpenNIUniformSampling
-{
- public:
- using Cloud = pcl::PointCloud<pcl::PointXYZRGBA>;
- using CloudPtr = Cloud::Ptr;
- using CloudConstPtr = Cloud::ConstPtr;
-
- OpenNIUniformSampling (const std::string& device_id = "",
- float leaf_size = 0.05)
- : viewer ("PCL OpenNI PassThrough Viewer")
- , device_id_(device_id)
- {
- pass_.setRadiusSearch (leaf_size);
- }
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
+
+class OpenNIUniformSampling {
+public:
+ using Cloud = pcl::PointCloud<pcl::PointXYZRGBA>;
+ using CloudPtr = Cloud::Ptr;
+ using CloudConstPtr = Cloud::ConstPtr;
+
+ OpenNIUniformSampling(const std::string& device_id = "", float leaf_size = 0.05)
+ : viewer("PCL OpenNI PassThrough Viewer"), device_id_(device_id)
+ {
+ pass_.setRadiusSearch(leaf_size);
+ }
- void
- cloud_cb_ (const CloudConstPtr& cloud)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- FPS_CALC ("computation");
-
- cloud_.reset (new Cloud);
- keypoints_.reset (new pcl::PointCloud<pcl::PointXYZ>);
- // Computation goes here
- pass_.setInputCloud (cloud);
- pcl::PointCloud<pcl::PointXYZRGBA> sampled;
- pass_.filter (sampled);
- *cloud_ = *cloud;
-
- pcl::copyPointCloud (sampled, *keypoints_);
+ void
+ cloud_cb_(const CloudConstPtr& cloud)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ FPS_CALC("computation");
+
+ cloud_.reset(new Cloud);
+ keypoints_.reset(new pcl::PointCloud<pcl::PointXYZ>);
+ // Computation goes here
+ pass_.setInputCloud(cloud);
+ pcl::PointCloud<pcl::PointXYZRGBA> sampled;
+ pass_.filter(sampled);
+ *cloud_ = *cloud;
+
+ pcl::copyPointCloud(sampled, *keypoints_);
+ }
+
+ void
+ viz_cb(pcl::visualization::PCLVisualizer& viz)
+ {
+ std::lock_guard<std::mutex> lock(mtx_);
+ if (!keypoints_ && !cloud_) {
+ std::this_thread::sleep_for(1s);
+ return;
}
- void
- viz_cb (pcl::visualization::PCLVisualizer& viz)
- {
- std::lock_guard<std::mutex> lock (mtx_);
- if (!keypoints_ && !cloud_)
- {
- std::this_thread::sleep_for(1s);
- return;
- }
-
- FPS_CALC ("visualization");
- viz.removePointCloud ("raw");
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color_handler (cloud_);
- viz.addPointCloud<pcl::PointXYZRGBA> (cloud_, color_handler, "raw");
-
- if (!viz.updatePointCloud<pcl::PointXYZ> (keypoints_, "keypoints"))
- {
- viz.addPointCloud<pcl::PointXYZ> (keypoints_, "keypoints");
- viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5.0, "keypoints");
- viz.resetCameraViewpoint ("keypoints");
- }
+ FPS_CALC("visualization");
+ viz.removePointCloud("raw");
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color_handler(
+ cloud_);
+ viz.addPointCloud<pcl::PointXYZRGBA>(cloud_, color_handler, "raw");
+
+ if (!viz.updatePointCloud<pcl::PointXYZ>(keypoints_, "keypoints")) {
+ viz.addPointCloud<pcl::PointXYZ>(keypoints_, "keypoints");
+ viz.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5.0, "keypoints");
+ viz.resetCameraViewpoint("keypoints");
}
+ }
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb_ (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
- viewer.runOnVisualizationThread ([this] (pcl::visualization::PCLVisualizer& viz) { viz_cb (viz); }, "viz_cb");
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
+ viewer.runOnVisualizationThread(
+ [this](pcl::visualization::PCLVisualizer& viz) { viz_cb(viz); }, "viz_cb");
- interface.start ();
-
- while (!viewer.wasStopped ())
- {
- std::this_thread::sleep_for(1s);
- }
+ interface.start();
- interface.stop ();
+ while (!viewer.wasStopped()) {
+ std::this_thread::sleep_for(1s);
}
- pcl::UniformSampling<pcl::PointXYZRGBA> pass_;
- pcl::visualization::CloudViewer viewer;
- std::string device_id_;
- std::mutex mtx_;
- CloudPtr cloud_;
- pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_;
+ interface.stop();
+ }
+
+ pcl::UniformSampling<pcl::PointXYZRGBA> pass_;
+ pcl::visualization::CloudViewer viewer;
+ std::string device_id_;
+ std::mutex mtx_;
+ CloudPtr cloud_;
+ pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n"
<< "where options are:\n"
- << " -leaf X :: set the UniformSampling leaf size (default: 0.01)\n";
+ << " -leaf X :: set the UniformSampling leaf "
+ "size (default: 0.01)\n";
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
std::cout << "No devices connected." << std::endl;
}
-int
-main (int argc, char ** argv)
+int
+main(int argc, char** argv)
{
- if (argc < 2)
- {
- usage (argv);
+ if (argc < 2) {
+ usage(argv);
return 1;
}
- std::string arg (argv[1]);
-
- if (arg == "--help" || arg == "-h")
- {
- usage (argv);
+ std::string arg(argv[1]);
+
+ if (arg == "--help" || arg == "-h") {
+ usage(argv);
return 1;
}
float leaf_res = 0.05f;
- pcl::console::parse_argument (argc, argv, "-leaf", leaf_res);
- PCL_INFO ("Using %f as a leaf size for UniformSampling.\n", leaf_res);
+ pcl::console::parse_argument(argc, argv, "-leaf", leaf_res);
+ PCL_INFO("Using %f as a leaf size for UniformSampling.\n", leaf_res);
- pcl::OpenNIGrabber grabber (arg);
- OpenNIUniformSampling v (arg, leaf_res);
- v.run ();
+ pcl::OpenNIGrabber grabber(arg);
+ OpenNIUniformSampling v(arg, leaf_res);
+ v.run();
- return (0);
+ return 0;
}
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
+ *
*/
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/voxel_grid.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
+#include <pcl/io/openni_camera/openni_driver.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <mutex>
+// clang-format off
+#define FPS_CALC(_WHAT_) \
+ do { \
+ static unsigned count = 0; \
+ static double last = pcl::getTime(); \
+ double now = pcl::getTime(); \
+ ++count; \
+ if (now - last >= 1.0) { \
+ std::cout << "Average framerate(" << _WHAT_ << "): " \
+ << double(count) / double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+ } while (false)
+// clang-format on
-#define FPS_CALC(_WHAT_) \
-do \
-{ \
- static unsigned count = 0;\
- static double last = pcl::getTime ();\
- double now = pcl::getTime (); \
- ++count; \
- if (now - last >= 1.0) \
- { \
- std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
- count = 0; \
- last = now; \
- } \
-}while(false)
+template <typename PointType>
+class OpenNIVoxelGrid {
+public:
+ using Cloud = pcl::PointCloud<PointType>;
+ using CloudPtr = typename Cloud::Ptr;
+ using CloudConstPtr = typename Cloud::ConstPtr;
+
+ OpenNIVoxelGrid(const std::string& device_id = "",
+ const std::string& = "z",
+ float = 0,
+ float = 5.0,
+ float leaf_size_x = 0.01,
+ float leaf_size_y = 0.01,
+ float leaf_size_z = 0.01)
+ : viewer("PCL OpenNI VoxelGrid Viewer"), device_id_(device_id)
+ {
+ grid_.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z);
+ // grid_.setFilterFieldName (field_name);
+ // grid_.setFilterLimits (min_v, max_v);
+ }
+ void
+ cloud_cb_(const CloudConstPtr& cloud)
+ {
+ set(cloud);
+ }
-template <typename PointType>
-class OpenNIVoxelGrid
-{
- public:
- using Cloud = pcl::PointCloud<PointType>;
- using CloudPtr = typename Cloud::Ptr;
- using CloudConstPtr = typename Cloud::ConstPtr;
-
- OpenNIVoxelGrid (const std::string& device_id = "",
- const std::string& = "z", float = 0, float = 5.0,
- float leaf_size_x = 0.01, float leaf_size_y = 0.01, float leaf_size_z = 0.01)
- : viewer ("PCL OpenNI VoxelGrid Viewer")
- , device_id_(device_id)
- {
- grid_.setLeafSize (leaf_size_x, leaf_size_y, leaf_size_z);
- //grid_.setFilterFieldName (field_name);
- //grid_.setFilterLimits (min_v, max_v);
- }
-
- void
- cloud_cb_ (const CloudConstPtr& cloud)
- {
- set (cloud);
- }
+ void
+ set(const CloudConstPtr& cloud)
+ {
+ // lock while we set our cloud;
+ std::lock_guard<std::mutex> lock(mtx_);
+ cloud_ = cloud;
+ }
- void
- set (const CloudConstPtr& cloud)
- {
- //lock while we set our cloud;
- std::lock_guard<std::mutex> lock (mtx_);
- cloud_ = cloud;
- }
+ CloudPtr
+ get()
+ {
+ // lock while we swap our cloud and reset it.
+ std::lock_guard<std::mutex> lock(mtx_);
+ CloudPtr temp_cloud(new Cloud);
- CloudPtr
- get ()
- {
- //lock while we swap our cloud and reset it.
- std::lock_guard<std::mutex> lock (mtx_);
- CloudPtr temp_cloud (new Cloud);
-
- grid_.setInputCloud (cloud_);
- grid_.filter (*temp_cloud);
-
- return (temp_cloud);
- }
+ grid_.setInputCloud(cloud_);
+ grid_.filter(*temp_cloud);
- void
- run ()
- {
- pcl::OpenNIGrabber interface {device_id_};
-
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb_ (cloud); };
- boost::signals2::connection c = interface.registerCallback (f);
-
- interface.start ();
-
- while (!viewer.wasStopped ())
- {
- if (cloud_)
- {
- FPS_CALC ("drawing");
- //the call to get() sets the cloud_ to null;
- viewer.showCloud (get ());
- }
- }
+ return temp_cloud;
+ }
+
+ void
+ run()
+ {
+ pcl::OpenNIGrabber interface{device_id_};
+
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb_(cloud);
+ };
+ boost::signals2::connection c = interface.registerCallback(f);
- interface.stop ();
+ interface.start();
+
+ while (!viewer.wasStopped()) {
+ if (cloud_) {
+ FPS_CALC("drawing");
+ // the call to get() sets the cloud_ to null;
+ viewer.showCloud(get());
+ }
}
- pcl::VoxelGrid<PointType> grid_;
- pcl::visualization::CloudViewer viewer;
- std::string device_id_;
- std::mutex mtx_;
- CloudConstPtr cloud_;
+ interface.stop();
+ }
+
+ pcl::VoxelGrid<PointType> grid_;
+ pcl::visualization::CloudViewer viewer;
+ std::string device_id_;
+ std::mutex mtx_;
+ CloudConstPtr cloud_;
};
void
-usage (char ** argv)
+usage(char** argv)
{
std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n"
- << "where options are:\n -minmax min-max :: set the ApproximateVoxelGrid min-max cutting values (default: 0-5.0)\n"
- << " -field X :: use field/dimension 'X' to filter data on (default: 'z')\n"
-
- << " -leaf x, y, z :: set the ApproximateVoxelGrid leaf size (default: 0.01)\n";
-
- openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- if (driver.getNumberDevices () > 0)
- {
- for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
- {
+ << "where options are:\n -minmax min-max :: set the "
+ "ApproximateVoxelGrid min-max cutting values (default: 0-5.0)\n"
+ << " -field X :: use field/dimension 'X' to filter data on "
+ "(default: 'z')\n"
+
+ << " -leaf x, y, z :: set the "
+ "ApproximateVoxelGrid leaf size (default: 0.01)\n";
+
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
+ if (driver.getNumberDevices() > 0) {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
+ // clang-format off
std::cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
<< ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << std::endl;
std::cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << std::endl
<< " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << std::endl
<< " <serial-number> (only in Linux and for devices which provide serial numbers)" << std::endl;
+ // clang-format on
}
}
else
std::cout << "No devices connected." << std::endl;
}
-int
-main (int argc, char ** argv)
+int
+main(int argc, char** argv)
{
- if (pcl::console::find_argument (argc, argv, "-h") != -1)
- usage (argv);
+ if (pcl::console::find_argument(argc, argv, "-h") != -1)
+ usage(argv);
float min_v = 0.0f, max_v = 5.0f;
- pcl::console::parse_2x_arguments (argc, argv, "-minmax", min_v, max_v);
- std::string field_name ("z");
- pcl::console::parse_argument (argc, argv, "-field", field_name);
- PCL_INFO ("Filtering data on %s between %f -> %f.\n", field_name.c_str (), min_v, max_v);
+ pcl::console::parse_2x_arguments(argc, argv, "-minmax", min_v, max_v);
+ std::string field_name("z");
+ pcl::console::parse_argument(argc, argv, "-field", field_name);
+ PCL_INFO(
+ "Filtering data on %s between %f -> %f.\n", field_name.c_str(), min_v, max_v);
float leaf_x = 0.01f, leaf_y = 0.01f, leaf_z = 0.01f;
- pcl::console::parse_3x_arguments (argc, argv, "-leaf", leaf_x, leaf_y, leaf_z);
- PCL_INFO ("Using %f, %f, %f as a leaf size for VoxelGrid.\n", leaf_x, leaf_y, leaf_z);
-
- pcl::OpenNIGrabber grabber ("");
- if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
- {
- OpenNIVoxelGrid<pcl::PointXYZRGBA> v ("", field_name, min_v, max_v, leaf_x, leaf_y, leaf_z);
- v.run ();
+ pcl::console::parse_3x_arguments(argc, argv, "-leaf", leaf_x, leaf_y, leaf_z);
+ PCL_INFO("Using %f, %f, %f as a leaf size for VoxelGrid.\n", leaf_x, leaf_y, leaf_z);
+
+ pcl::OpenNIGrabber grabber("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
+ OpenNIVoxelGrid<pcl::PointXYZRGBA> v(
+ "", field_name, min_v, max_v, leaf_x, leaf_y, leaf_z);
+ v.run();
}
- else
- {
- OpenNIVoxelGrid<pcl::PointXYZ> v ("", field_name, min_v, max_v, leaf_x, leaf_y, leaf_z);
- v.run ();
+ else {
+ OpenNIVoxelGrid<pcl::PointXYZ> v(
+ "", field_name, min_v, max_v, leaf_x, leaf_y, leaf_z);
+ v.run();
}
- return (0);
+ return 0;
}
-#include <pcl/common/angles.h>
#include <pcl/apps/organized_segmentation_demo.h>
-//QT4
+#include <pcl/common/angles.h>
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl/memory.h> // for pcl::dynamic_pointer_cast
+
+#include <boost/signals2/connection.hpp> // for boost::signals2::connection
+
#include <QApplication>
-#include <QMutexLocker>
#include <QEvent>
+#include <QMutexLocker>
#include <QObject>
-#include <pcl/segmentation/extract_polygonal_prism_data.h>
-#include <pcl/surface/convex_hull.h>
-
#include <vtkRenderWindow.h>
+// #include <boost/filesystem.hpp> // for boost::filesystem::directory_iterator
+#include <boost/signals2/connection.hpp> // for boost::signals2::connection
+
void
-displayPlanarRegions (std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > ®ions,
- const pcl::visualization::PCLVisualizer::Ptr& viewer)
+displayPlanarRegions(
+ std::vector<pcl::PlanarRegion<PointT>,
+ Eigen::aligned_allocator<pcl::PlanarRegion<PointT>>>& regions,
+ const pcl::visualization::PCLVisualizer::Ptr& viewer)
{
char name[1024];
- unsigned char red [6] = {255, 0, 0, 255, 255, 0};
- unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
- unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
-
- pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
-
- for (std::size_t i = 0; i < regions.size (); i++)
- {
- Eigen::Vector3f centroid = regions[i].getCentroid ();
- Eigen::Vector4f model = regions[i].getCoefficients ();
- pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
- pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
- centroid[1] + (0.5f * model[1]),
- centroid[2] + (0.5f * model[2]));
- sprintf (name, "normal_%d", unsigned (i));
- viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name);
-
- contour->points = regions[i].getContour ();
- sprintf (name, "plane_%02d", int (i));
- pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i%6], grn[i%6], blu[i%6]);
- if(!viewer->updatePointCloud(contour, color, name))
- viewer->addPointCloud (contour, color, name);
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
+ unsigned char red[6] = {255, 0, 0, 255, 255, 0};
+ unsigned char grn[6] = {0, 255, 0, 255, 0, 255};
+ unsigned char blu[6] = {0, 0, 255, 0, 255, 255};
+
+ pcl::PointCloud<PointT>::Ptr contour(new pcl::PointCloud<PointT>);
+
+ for (std::size_t i = 0; i < regions.size(); i++) {
+ Eigen::Vector3f centroid = regions[i].getCentroid();
+ Eigen::Vector4f model = regions[i].getCoefficients();
+ pcl::PointXYZ pt1 = pcl::PointXYZ(centroid[0], centroid[1], centroid[2]);
+ pcl::PointXYZ pt2 = pcl::PointXYZ(centroid[0] + (0.5f * model[0]),
+ centroid[1] + (0.5f * model[1]),
+ centroid[2] + (0.5f * model[2]));
+ sprintf(name, "normal_%d", unsigned(i));
+ viewer->addArrow(pt2, pt1, 1.0, 0, 0, false, name);
+
+ contour->points = regions[i].getContour();
+ sprintf(name, "plane_%02d", int(i));
+ pcl::visualization::PointCloudColorHandlerCustom<PointT> color(
+ contour, red[i % 6], grn[i % 6], blu[i % 6]);
+ if (!viewer->updatePointCloud(contour, color, name))
+ viewer->addPointCloud(contour, color, name);
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
}
}
void
-displayEuclideanClusters (const pcl::PointCloud<PointT>::CloudVectorType &clusters,
- const pcl::visualization::PCLVisualizer::Ptr& viewer)
+displayEuclideanClusters(const pcl::PointCloud<PointT>::CloudVectorType& clusters,
+ const pcl::visualization::PCLVisualizer::Ptr& viewer)
{
char name[1024];
- unsigned char red [6] = {255, 0, 0, 255, 255, 0};
- unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
- unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
-
- for (std::size_t i = 0; i < clusters.size (); i++)
- {
- sprintf (name, "cluster_%d" , int (i));
- pcl::PointCloud<PointT>::ConstPtr cluster_cloud (new pcl::PointCloud<PointT> (clusters[i]));
- pcl::visualization::PointCloudColorHandlerCustom<PointT> color0(cluster_cloud,red[i%6],grn[i%6],blu[i%6]);
- if (!viewer->updatePointCloud (cluster_cloud,color0,name))
- viewer->addPointCloud (cluster_cloud,color0,name);
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, name);
+ unsigned char red[6] = {255, 0, 0, 255, 255, 0};
+ unsigned char grn[6] = {0, 255, 0, 255, 0, 255};
+ unsigned char blu[6] = {0, 0, 255, 0, 255, 255};
+
+ for (std::size_t i = 0; i < clusters.size(); i++) {
+ sprintf(name, "cluster_%d", int(i));
+ pcl::PointCloud<PointT>::ConstPtr cluster_cloud(
+ new pcl::PointCloud<PointT>(clusters[i]));
+ pcl::visualization::PointCloudColorHandlerCustom<PointT> color0(
+ cluster_cloud, red[i % 6], grn[i % 6], blu[i % 6]);
+ if (!viewer->updatePointCloud(cluster_cloud, color0, name))
+ viewer->addPointCloud(cluster_cloud, color0, name);
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, name);
}
}
void
-displayCurvature (pcl::PointCloud<PointT>& cloud, pcl::PointCloud<pcl::Normal>& normals, const pcl::visualization::PCLVisualizer::Ptr& viewer)
+displayCurvature(pcl::PointCloud<PointT>& cloud,
+ pcl::PointCloud<pcl::Normal>& normals,
+ const pcl::visualization::PCLVisualizer::Ptr& viewer)
{
pcl::PointCloud<pcl::PointXYZRGBA> curvature_cloud = cloud;
- for (std::size_t i = 0; i < cloud.points.size (); i++)
- {
- if (normals.points[i].curvature < 0.04)
- {
+ for (std::size_t i = 0; i < cloud.points.size(); i++) {
+ if (normals.points[i].curvature < 0.04) {
curvature_cloud.points[i].r = 0;
curvature_cloud.points[i].g = 255;
curvature_cloud.points[i].b = 0;
}
- else
- {
+ else {
curvature_cloud.points[i].r = 255;
curvature_cloud.points[i].g = 0;
curvature_cloud.points[i].b = 0;
}
}
- if (!viewer->updatePointCloud (curvature_cloud.makeShared (), "curvature"))
- viewer->addPointCloud (curvature_cloud.makeShared (), "curvature");
+ if (!viewer->updatePointCloud(curvature_cloud.makeShared(), "curvature"))
+ viewer->addPointCloud(curvature_cloud.makeShared(), "curvature");
}
void
-displayDistanceMap (pcl::PointCloud<PointT>& cloud, float* distance_map, const pcl::visualization::PCLVisualizer::Ptr& viewer)
+displayDistanceMap(pcl::PointCloud<PointT>& cloud,
+ float* distance_map,
+ const pcl::visualization::PCLVisualizer::Ptr& viewer)
{
pcl::PointCloud<pcl::PointXYZRGBA> distance_map_cloud = cloud;
- for (std::size_t i = 0; i < cloud.points.size (); i++)
- {
- if (distance_map[i] < 5.0)
- {
+ for (std::size_t i = 0; i < cloud.points.size(); i++) {
+ if (distance_map[i] < 5.0) {
distance_map_cloud.points[i].r = 255;
distance_map_cloud.points[i].g = 0;
distance_map_cloud.points[i].b = 0;
}
- else
- {
+ else {
distance_map_cloud.points[i].r = 0;
distance_map_cloud.points[i].g = 255;
distance_map_cloud.points[i].b = 0;
}
}
- if (!viewer->updatePointCloud (distance_map_cloud.makeShared (), "distance_map"))
- viewer->addPointCloud (distance_map_cloud.makeShared (), "distance_map");
+ if (!viewer->updatePointCloud(distance_map_cloud.makeShared(), "distance_map"))
+ viewer->addPointCloud(distance_map_cloud.makeShared(), "distance_map");
}
void
-removePreviousDataFromScreen (std::size_t prev_models_size, std::size_t prev_clusters_size, const pcl::visualization::PCLVisualizer::Ptr& viewer)
+removePreviousDataFromScreen(std::size_t prev_models_size,
+ std::size_t prev_clusters_size,
+ const pcl::visualization::PCLVisualizer::Ptr& viewer)
{
char name[1024];
- for (std::size_t i = 0; i < prev_models_size; i++)
- {
- sprintf (name, "normal_%d", unsigned (i));
- viewer->removeShape (name);
+ for (std::size_t i = 0; i < prev_models_size; i++) {
+ sprintf(name, "normal_%d", unsigned(i));
+ viewer->removeShape(name);
- sprintf (name, "plane_%02d", int (i));
- viewer->removePointCloud (name);
+ sprintf(name, "plane_%02d", int(i));
+ viewer->removePointCloud(name);
}
- for (std::size_t i = 0; i < prev_clusters_size; i++)
- {
- sprintf (name, "cluster_%d", int (i));
- viewer->removePointCloud (name);
+ for (std::size_t i = 0; i < prev_clusters_size; i++) {
+ sprintf(name, "cluster_%d", int(i));
+ viewer->removePointCloud(name);
}
}
bool
-compareClusterToRegion (pcl::PlanarRegion<PointT>& region, pcl::PointCloud<PointT>& cluster)
+compareClusterToRegion(pcl::PlanarRegion<PointT>& region,
+ pcl::PointCloud<PointT>& cluster)
{
- Eigen::Vector4f model = region.getCoefficients ();
+ Eigen::Vector4f model = region.getCoefficients();
pcl::PointCloud<PointT> poly;
- poly.points = region.getContour ();
+ poly.points = region.getContour();
- for (const auto &point : cluster.points)
- {
- double ptp_dist = std::abs (model[0] * point.x +
- model[1] * point.y +
- model[2] * point.z +
- model[3]);
- bool in_poly = pcl::isPointIn2DPolygon<PointT> (point, poly);
+ for (const auto& point : cluster.points) {
+ double ptp_dist = std::abs(model[0] * point.x + model[1] * point.y +
+ model[2] * point.z + model[3]);
+ bool in_poly = pcl::isPointIn2DPolygon<PointT>(point, poly);
if (in_poly && ptp_dist < 0.02)
return true;
}
}
bool
-comparePointToRegion (PointT& pt, pcl::ModelCoefficients& model, pcl::PointCloud<PointT>& poly)
+comparePointToRegion(PointT& pt,
+ pcl::ModelCoefficients& model,
+ pcl::PointCloud<PointT>& poly)
{
- //bool dist_ok;
-
- double ptp_dist = std::abs (model.values[0] * pt.x +
- model.values[1] * pt.y +
- model.values[2] * pt.z +
- model.values[3]);
+ double ptp_dist = std::abs(model.values[0] * pt.x + model.values[1] * pt.y +
+ model.values[2] * pt.z + model.values[3]);
if (ptp_dist >= 0.1)
- return (false);
-// else
-// dist_ok = true;
+ return false;
- //project the point onto the plane
- Eigen::Vector3f mc (model.values[0], model.values[1], model.values[2]);
+ // project the point onto the plane
+ Eigen::Vector3f mc(model.values[0], model.values[1], model.values[2]);
Eigen::Vector3f pt_vec;
pt_vec[0] = pt.x;
pt_vec[1] = pt.y;
pt_vec[2] = pt.z;
- Eigen::Vector3f projected (pt_vec - mc * float (ptp_dist));
+ Eigen::Vector3f projected(pt_vec - mc * float(ptp_dist));
PointT projected_pt;
projected_pt.x = projected[0];
projected_pt.y = projected[1];
projected_pt.z = projected[2];
- PCL_INFO ("pt: %lf %lf %lf\n", projected_pt.x, projected_pt.y, projected_pt.z);
+ PCL_INFO("pt: %lf %lf %lf\n", projected_pt.x, projected_pt.y, projected_pt.z);
- if (pcl::isPointIn2DPolygon (projected_pt, poly))
- {
- PCL_INFO ("inside!\n");
+ if (pcl::isPointIn2DPolygon(projected_pt, poly)) {
+ PCL_INFO("inside!\n");
return true;
}
- PCL_INFO ("not inside!\n");
+ PCL_INFO("not inside!\n");
return false;
-
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-OrganizedSegmentationDemo::OrganizedSegmentationDemo (pcl::Grabber& grabber) : grabber_ (grabber)
+OrganizedSegmentationDemo::OrganizedSegmentationDemo(pcl::Grabber& grabber)
+: grabber_(grabber)
{
- //Create a timer
- vis_timer_ = new QTimer (this);
- vis_timer_->start (5);//5ms
+ // Create a timer
+ vis_timer_ = new QTimer(this);
+ vis_timer_->start(5); // 5ms
- connect (vis_timer_, SIGNAL (timeout ()), this, SLOT (timeoutSlot()));
+ connect(vis_timer_, SIGNAL(timeout()), this, SLOT(timeoutSlot()));
ui_ = new Ui::MainWindow;
- ui_->setupUi (this);
-
- this->setWindowTitle ("PCL Organized Connected Component Segmentation Demo");
- vis_.reset (new pcl::visualization::PCLVisualizer ("", false));
- ui_->qvtk_widget->SetRenderWindow (vis_->getRenderWindow ());
- vis_->setupInteractor (ui_->qvtk_widget->GetInteractor (), ui_->qvtk_widget->GetRenderWindow ());
- vis_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
- ui_->qvtk_widget->update ();
+ ui_->setupUi(this);
+
+ this->setWindowTitle("PCL Organized Connected Component Segmentation Demo");
+ vis_.reset(new pcl::visualization::PCLVisualizer("", false));
+ ui_->qvtk_widget->SetRenderWindow(vis_->getRenderWindow());
+ vis_->setupInteractor(ui_->qvtk_widget->GetInteractor(),
+ ui_->qvtk_widget->GetRenderWindow());
+ vis_->getInteractorStyle()->setKeyboardModifier(
+ pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+ ui_->qvtk_widget->update();
- std::function<void (const CloudConstPtr&)> f = [this] (const CloudConstPtr& cloud) { cloud_cb (cloud); };
+ std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
+ cloud_cb(cloud);
+ };
boost::signals2::connection c = grabber_.registerCallback(f);
- connect (ui_->captureButton, SIGNAL(clicked()), this, SLOT(toggleCapturePressed()));
-
- connect (ui_->euclideanComparatorButton, SIGNAL (clicked ()), this, SLOT (useEuclideanComparatorPressed ()));
- connect (ui_->planeComparatorButton, SIGNAL (clicked ()), this, SLOT (usePlaneComparatorPressed ()));
- connect (ui_->rgbComparatorButton, SIGNAL (clicked ()), this, SLOT (useRGBComparatorPressed ()));
- connect (ui_->edgeAwareComparatorButton, SIGNAL (clicked ()), this, SLOT (useEdgeAwareComparatorPressed ()));
-
- connect (ui_->displayCurvatureButton, SIGNAL (clicked ()), this, SLOT (displayCurvaturePressed ()));
- connect (ui_->displayDistanceMapButton, SIGNAL (clicked ()), this, SLOT (displayDistanceMapPressed ()));
- connect (ui_->displayNormalsButton, SIGNAL (clicked ()), this, SLOT (displayNormalsPressed ()));
-
- connect (ui_->disableRefinementButton, SIGNAL (clicked ()), this, SLOT (disableRefinementPressed ()));
- connect (ui_->planarRefinementButton, SIGNAL (clicked ()), this, SLOT (usePlanarRefinementPressed ()));
-
- connect (ui_->disableClusteringButton, SIGNAL (clicked ()), this, SLOT (disableClusteringPressed ()));
- connect (ui_->euclideanClusteringButton, SIGNAL (clicked ()), this, SLOT (useEuclideanClusteringPressed ()));
+ connect(ui_->captureButton, SIGNAL(clicked()), this, SLOT(toggleCapturePressed()));
+
+ connect(ui_->euclideanComparatorButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(useEuclideanComparatorPressed()));
+ connect(ui_->planeComparatorButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(usePlaneComparatorPressed()));
+ connect(ui_->rgbComparatorButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(useRGBComparatorPressed()));
+ connect(ui_->edgeAwareComparatorButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(useEdgeAwareComparatorPressed()));
+
+ connect(ui_->displayCurvatureButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(displayCurvaturePressed()));
+ connect(ui_->displayDistanceMapButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(displayDistanceMapPressed()));
+ connect(ui_->displayNormalsButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(displayNormalsPressed()));
+
+ connect(ui_->disableRefinementButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(disableRefinementPressed()));
+ connect(ui_->planarRefinementButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(usePlanarRefinementPressed()));
+
+ connect(ui_->disableClusteringButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(disableClusteringPressed()));
+ connect(ui_->euclideanClusteringButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(useEuclideanClusteringPressed()));
capture_ = false;
previous_data_size_ = 0;
use_planar_refinement_ = true;
use_clustering_ = false;
-
// Set up Normal Estimation
- //ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
- ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
- ne.setMaxDepthChangeFactor (0.02f); // set as default, well performing for tabletop objects as imaged by a primesense sensor
- ne.setNormalSmoothingSize (20.0f);
-
- plane_comparator_.reset (new pcl::PlaneCoefficientComparator<PointT, pcl::Normal> ());
- euclidean_comparator_.reset (new pcl::EuclideanPlaneCoefficientComparator<PointT, pcl::Normal> ());
- rgb_comparator_.reset (new pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal> ());
- edge_aware_comparator_.reset (new pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal> ());
- euclidean_cluster_comparator_ = pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr (new pcl::EuclideanClusterComparator<PointT, pcl::Label> ());
+ // ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
+ ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
+ ne.setMaxDepthChangeFactor(0.02f); // set as default, well performing for tabletop
+ // objects as imaged by a primesense sensor
+ ne.setNormalSmoothingSize(20.0f);
+
+ plane_comparator_.reset(new pcl::PlaneCoefficientComparator<PointT, pcl::Normal>());
+ euclidean_comparator_.reset(
+ new pcl::EuclideanPlaneCoefficientComparator<PointT, pcl::Normal>());
+ rgb_comparator_.reset(new pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal>());
+ edge_aware_comparator_.reset(
+ new pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>());
+ euclidean_cluster_comparator_ =
+ pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr(
+ new pcl::EuclideanClusterComparator<PointT, pcl::Label>());
// Set up Organized Multi Plane Segmentation
- mps.setMinInliers (10000u);
- mps.setAngularThreshold (pcl::deg2rad (3.0)); // 3 degrees, set as default, well performing for tabletop objects as imaged by a primesense sensor
- mps.setDistanceThreshold (0.02); // 2cm, set as default, well performing for tabletop objects as imaged by a primesense sensor
-
-
- PCL_INFO ("starting grabber\n");
- grabber_.start ();
+ mps.setMinInliers(10000u);
+ mps.setAngularThreshold(
+ pcl::deg2rad(3.0)); // 3 degrees, set as default, well performing for tabletop
+ // objects as imaged by a primesense sensor
+ mps.setDistanceThreshold(0.02); // 2cm, set as default, well performing for tabletop
+ // objects as imaged by a primesense sensor
+
+ PCL_INFO("starting grabber\n");
+ grabber_.start();
}
void
-OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud)
+OrganizedSegmentationDemo::cloud_cb(const CloudConstPtr& cloud)
{
if (!capture_)
return;
- QMutexLocker locker (&mtx_);
- FPS_CALC ("computation");
+ QMutexLocker locker(&mtx_);
+ FPS_CALC("computation");
// Estimate Normals
- pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
- ne.setInputCloud (cloud);
- ne.compute (*normal_cloud);
- float* distance_map = ne.getDistanceMap ();
- pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>::Ptr eapc = boost::dynamic_pointer_cast<pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal> >(edge_aware_comparator_);
- eapc->setDistanceMap (distance_map);
- eapc->setDistanceThreshold (0.01f, false);
+ pcl::PointCloud<pcl::Normal>::Ptr normal_cloud(new pcl::PointCloud<pcl::Normal>);
+ ne.setInputCloud(cloud);
+ ne.compute(*normal_cloud);
+ float* distance_map = ne.getDistanceMap();
+ pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>::Ptr eapc =
+ pcl::dynamic_pointer_cast<pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>>(
+ edge_aware_comparator_);
+ eapc->setDistanceMap(distance_map);
+ eapc->setDistanceThreshold(0.01f, false);
// Segment Planes
- double mps_start = pcl::getTime ();
- std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
+ double mps_start = pcl::getTime();
+ std::vector<pcl::PlanarRegion<PointT>,
+ Eigen::aligned_allocator<pcl::PlanarRegion<PointT>>>
+ regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
- pcl::PointCloud<pcl::Label>::Ptr labels
(new pcl::PointCloud<pcl::Label>);
+ pcl::PointCloud<pcl::Label>::Ptr labels(new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
- mps.setInputNormals (normal_cloud);
- mps.setInputCloud (cloud);
- if (use_planar_refinement_)
- {
- mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
+ mps.setInputNormals(normal_cloud);
+ mps.setInputCloud(cloud);
+ if (use_planar_refinement_) {
+ mps.segmentAndRefine(regions,
+ model_coefficients,
+ inlier_indices,
+ labels,
+ label_indices,
+ boundary_indices);
}
- else
- {
- mps.segment (regions);//, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
+ else {
+ mps.segment(regions);
}
- double mps_end = pcl::getTime ();
+ double mps_end = pcl::getTime();
std::cout << "MPS+Refine took: " << double(mps_end - mps_start) << std::endl;
- //Segment Objects
+ // Segment Objects
pcl::PointCloud<PointT>::CloudVectorType clusters;
- if (use_clustering_ && !regions.empty ())
- {
- pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSetPtr plane_labels (new pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSet);
- for (std::size_t i = 0; i < label_indices.size (); ++i)
- if (label_indices[i].indices.size () > mps.getMinInliers())
- plane_labels->insert (i);
+ if (use_clustering_ && !regions.empty()) {
+ pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSetPtr
+ plane_labels(
+ new pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSet);
+ for (std::size_t i = 0; i < label_indices.size(); ++i)
+ if (label_indices[i].indices.size() > mps.getMinInliers())
+ plane_labels->insert(i);
- euclidean_cluster_comparator_->setInputCloud (cloud);
- euclidean_cluster_comparator_->setLabels (labels);
- euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
- euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false);
+ euclidean_cluster_comparator_->setInputCloud(cloud);
+ euclidean_cluster_comparator_->setLabels(labels);
+ euclidean_cluster_comparator_->setExcludeLabels(plane_labels);
+ euclidean_cluster_comparator_->setDistanceThreshold(0.01f, false);
pcl::PointCloud<pcl::Label> euclidean_labels;
std::vector<pcl::PointIndices> euclidean_label_indices;
- pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
- euclidean_segmentation.setInputCloud (cloud);
- euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
-
- for (const auto &euclidean_label_index : euclidean_label_indices)
- {
- if (euclidean_label_index.indices.size () > 1000u)
- {
+ pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label>
+ euclidean_segmentation(euclidean_cluster_comparator_);
+ euclidean_segmentation.setInputCloud(cloud);
+ euclidean_segmentation.segment(euclidean_labels, euclidean_label_indices);
+
+ for (const auto& euclidean_label_index : euclidean_label_indices) {
+ if (euclidean_label_index.indices.size() > 1000u) {
pcl::PointCloud<PointT> cluster;
- pcl::copyPointCloud (*cloud, euclidean_label_index.indices,cluster);
- clusters.push_back (cluster);
+ pcl::copyPointCloud(*cloud, euclidean_label_index.indices, cluster);
+ clusters.push_back(cluster);
}
}
- PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ());
+ PCL_INFO("Got %d euclidean clusters!\n", clusters.size());
}
{
- QMutexLocker vis_locker (&vis_mtx_);
+ QMutexLocker vis_locker(&vis_mtx_);
prev_cloud_ = *cloud;
prev_normals_ = *normal_cloud;
prev_regions_ = regions;
}
void
-OrganizedSegmentationDemo::timeoutSlot ()
+OrganizedSegmentationDemo::timeoutSlot()
{
{
- QMutexLocker vis_locker (&vis_mtx_);
- if (capture_ && data_modified_)
- {
- removePreviousDataFromScreen (previous_data_size_, previous_clusters_size_, vis_);
- if (!vis_->updatePointCloud (prev_cloud_.makeShared (), "cloud"))
- {
- vis_->addPointCloud (prev_cloud_.makeShared (), "cloud");
- vis_->resetCameraViewpoint ("cloud");
+ QMutexLocker vis_locker(&vis_mtx_);
+ if (capture_ && data_modified_) {
+ removePreviousDataFromScreen(previous_data_size_, previous_clusters_size_, vis_);
+ if (!vis_->updatePointCloud(prev_cloud_.makeShared(), "cloud")) {
+ vis_->addPointCloud(prev_cloud_.makeShared(), "cloud");
+ vis_->resetCameraViewpoint("cloud");
}
- displayPlanarRegions (prev_regions_, vis_);
+ displayPlanarRegions(prev_regions_, vis_);
if (display_curvature_)
- displayCurvature (prev_cloud_, prev_normals_, vis_);
+ displayCurvature(prev_cloud_, prev_normals_, vis_);
else
- vis_->removePointCloud ("curvature");
+ vis_->removePointCloud("curvature");
if (display_distance_map_)
- displayDistanceMap (prev_cloud_, prev_distance_map_, vis_);
+ displayDistanceMap(prev_cloud_, prev_distance_map_, vis_);
else
- vis_->removePointCloud ("distance_map");
-
- if (display_normals_)
- {
- vis_->removePointCloud ("normals");
- vis_->addPointCloudNormals<PointT,pcl::Normal>(prev_cloud_.makeShared(), prev_normals_.makeShared (), 10, 0.05f, "normals");
- vis_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "normals");
+ vis_->removePointCloud("distance_map");
+
+ if (display_normals_) {
+ vis_->removePointCloud("normals");
+ vis_->addPointCloudNormals<PointT, pcl::Normal>(
+ prev_cloud_.makeShared(), prev_normals_.makeShared(), 10, 0.05f, "normals");
+ vis_->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "normals");
}
- else
- {
- vis_->removePointCloud ("normals");
+ else {
+ vis_->removePointCloud("normals");
}
- displayEuclideanClusters (prev_clusters_,vis_);
+ displayEuclideanClusters(prev_clusters_, vis_);
- previous_data_size_ = prev_regions_.size ();
- previous_clusters_size_ = prev_clusters_.size ();
+ previous_data_size_ = prev_regions_.size();
+ previous_clusters_size_ = prev_clusters_.size();
data_modified_ = false;
}
}
}
void
-OrganizedSegmentationDemo::useEuclideanComparatorPressed ()
+OrganizedSegmentationDemo::useEuclideanComparatorPressed()
{
- QMutexLocker locker (&mtx_);
- PCL_INFO ("Setting Comparator to Euclidean\n");
- mps.setComparator (euclidean_comparator_);
+ QMutexLocker locker(&mtx_);
+ PCL_INFO("Setting Comparator to Euclidean\n");
+ mps.setComparator(euclidean_comparator_);
}
void
-OrganizedSegmentationDemo::useRGBComparatorPressed ()
+OrganizedSegmentationDemo::useRGBComparatorPressed()
{
- QMutexLocker locker (&mtx_);
- PCL_INFO ("Setting Comparator to RGB\n");
- mps.setComparator (rgb_comparator_);
+ QMutexLocker locker(&mtx_);
+ PCL_INFO("Setting Comparator to RGB\n");
+ mps.setComparator(rgb_comparator_);
}
void
-OrganizedSegmentationDemo::usePlaneComparatorPressed ()
+OrganizedSegmentationDemo::usePlaneComparatorPressed()
{
- QMutexLocker locker (&mtx_);
- PCL_INFO ("Setting Comparator to Plane\n");
- mps.setComparator (plane_comparator_);
+ QMutexLocker locker(&mtx_);
+ PCL_INFO("Setting Comparator to Plane\n");
+ mps.setComparator(plane_comparator_);
}
void
-OrganizedSegmentationDemo::useEdgeAwareComparatorPressed ()
+OrganizedSegmentationDemo::useEdgeAwareComparatorPressed()
{
- QMutexLocker locker (&mtx_);
- PCL_INFO ("Setting Comparator to edge aware\n");
- mps.setComparator (edge_aware_comparator_);
+ QMutexLocker locker(&mtx_);
+ PCL_INFO("Setting Comparator to edge aware\n");
+ mps.setComparator(edge_aware_comparator_);
}
void
-OrganizedSegmentationDemo::displayCurvaturePressed ()
+OrganizedSegmentationDemo::displayCurvaturePressed()
{
display_curvature_ = !display_curvature_;
}
void
-OrganizedSegmentationDemo::displayDistanceMapPressed ()
+OrganizedSegmentationDemo::displayDistanceMapPressed()
{
display_distance_map_ = !display_distance_map_;
}
void
-OrganizedSegmentationDemo::displayNormalsPressed ()
+OrganizedSegmentationDemo::displayNormalsPressed()
{
display_normals_ = !display_normals_;
}
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
QApplication app(argc, argv);
- //PCL_INFO ("Creating PCD Grabber\n");
- //std::vector<std::string> pcd_files;
- //boost::filesystem::directory_iterator end_itr;
- //for (boost::filesystem::directory_iterator itr("/u/atrevor/data/sushi_long_pcds_compressed"); itr != end_itr; ++itr)
- //{
- // pcd_files.push_back (itr->path ().string ());
- // std::cout << "added: " << itr->path ().string () << std::endl;
- //}
- //sort (pcd_files.begin (),pcd_files.end ());
-
- //pcl::PCDGrabber<pcl::PointXYZRGB> grabber(pcd_files,5.0,false);
- //PCL_INFO ("PCD Grabber created\n");
-
- pcl::OpenNIGrabber grabber ("#1");
+ pcl::OpenNIGrabber grabber("#1");
- OrganizedSegmentationDemo seg_demo (grabber);
+ OrganizedSegmentationDemo seg_demo(grabber);
seg_demo.show();
- return (QApplication::exec ());
+ return QApplication::exec();
}
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
- *
+ *
*
*/
-#include <thread>
-
-#include <pcl/console/print.h>
#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
#include <pcl/console/time.h>
-#include <pcl/features/organized_edge_detection.h>
#include <pcl/features/integral_image_normal.h>
+#include <pcl/features/organized_edge_detection.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/PCLPointCloud2.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <thread>
using namespace pcl;
using namespace pcl::io;
using namespace std::chrono_literals;
float default_th_dd = 0.02f;
-int default_max_search = 50;
+int default_max_search = 50;
using Cloud = pcl::PointCloud<pcl::PointXYZRGBA>;
using CloudPtr = Cloud::Ptr;
using CloudConstPtr = Cloud::ConstPtr;
-pcl::visualization::PCLVisualizer viewer ("3D Edge Viewer");
+pcl::visualization::PCLVisualizer viewer("3D Edge Viewer");
void
-printHelp (int, char **argv)
+printHelp(int, char** argv)
{
+ // clang-format off
print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
print_info (" where options are:\n");
print_info (" -th_dd X = the tolerance in meters for difference in depth values between neighboring points (The value is set for 1 meter and is adapted with respect to depth value linearly. (e.g. 2.0*th_dd in 2 meter depth)) (default: ");
print_value ("%f", default_th_dd); print_info (")\n");
print_info (" -max_search X = the max search distance for deciding occluding and occluded edges (default: ");
print_value ("%d", default_max_search); print_info (")\n");
+ // clang-format on
}
bool
-loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud)
+loadCloud(const std::string& filename, pcl::PCLPointCloud2& cloud)
{
TicToc tt;
- print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
-
- tt.tic ();
- if (loadPCDFile (filename, cloud) < 0)
- return (false);
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
- print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
-
- return (true);
+ print_highlight("Loading ");
+ print_value("%s ", filename.c_str());
+
+ tt.tic();
+ if (loadPCDFile(filename, cloud) < 0)
+ return false;
+ print_info("[done, ");
+ print_value("%g", tt.toc());
+ print_info(" ms : ");
+ print_value("%d", cloud.width * cloud.height);
+ print_info(" points]\n");
+ print_info("Available dimensions: ");
+ print_value("%s\n", pcl::getFieldsList(cloud).c_str());
+
+ return true;
}
void
-saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output)
+saveCloud(const std::string& filename, const pcl::PCLPointCloud2& output)
{
TicToc tt;
- tt.tic ();
-
- print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
-
- pcl::io::savePCDFile (filename, output, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true); // Save as binary
-
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
+ tt.tic();
+
+ print_highlight("Saving ");
+ print_value("%s ", filename.c_str());
+
+ pcl::io::savePCDFile(filename,
+ output,
+ Eigen::Vector4f::Zero(),
+ Eigen::Quaternionf::Identity(),
+ true); // Save as binary
+
+ print_info("[done, ");
+ print_value("%g", tt.toc());
+ print_info(" ms : ");
+ print_value("%d", output.width * output.height);
+ print_info(" points]\n");
}
-void
-keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
+void
+keyboard_callback(const pcl::visualization::KeyboardEvent& event, void*)
{
- if (event.keyUp())
- {
+ if (event.keyUp()) {
double opacity;
- switch (event.getKeyCode())
- {
- case '1':
- viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges");
- break;
- case '2':
- viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges");
- break;
- case '3':
- viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges");
- break;
- case '4':
- viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges");
- break;
- case '5':
- viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges");
- break;
+ switch (event.getKeyCode()) {
+ case '1':
+ viewer.getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY,
+ 1.0 - opacity,
+ "nan boundary edges");
+ break;
+ case '2':
+ viewer.getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - opacity, "occluding edges");
+ break;
+ case '3':
+ viewer.getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - opacity, "occluded edges");
+ break;
+ case '4':
+ viewer.getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY,
+ 1.0 - opacity,
+ "high curvature edges");
+ break;
+ case '5':
+ viewer.getPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - opacity, "rgb edges");
+ break;
}
}
}
void
-compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
- float th_dd, int max_search)
+compute(const pcl::PCLPointCloud2::ConstPtr& input,
+ pcl::PCLPointCloud2& output,
+ float th_dd,
+ int max_search)
{
- CloudPtr cloud (new Cloud);
- fromPCLPointCloud2 (*input, *cloud);
+ CloudPtr cloud(new Cloud);
+ fromPCLPointCloud2(*input, *cloud);
- pcl::PointCloud<pcl::Normal>::Ptr normal
(new pcl::PointCloud<pcl::Normal>);
+ pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
- ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
- ne.setNormalSmoothingSize (10.0f);
- ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
- ne.setInputCloud (cloud);
- ne.compute (*normal);
+ ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
+ ne.setNormalSmoothingSize(10.0f);
+ ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
+ ne.setInputCloud(cloud);
+ ne.compute(*normal);
TicToc tt;
- tt.tic ();
+ tt.tic();
- //OrganizedEdgeBase<PointXYZRGBA, Label> oed;
- //OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
- //OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
+ //
OrganizedEdgeBase<PointXYZRGBA, Label> oed;
+ //
OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
+ //
OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
- oed.setInputNormals (normal);
- oed.setInputCloud (cloud);
- oed.setDepthDisconThreshold (th_dd);
- oed.setMaxSearchNeighbors (max_search);
- oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
+ oed.setInputNormals(normal);
+ oed.setInputCloud(cloud);
+ oed.setDepthDisconThreshold(th_dd);
+ oed.setMaxSearchNeighbors(max_search);
+ oed.setEdgeType(oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING |
+ oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE |
+ oed.EDGELABEL_RGB_CANNY);
PointCloud<Label> labels;
std::vector<PointIndices> label_indices;
- oed.compute (labels, label_indices);
- print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
+ oed.compute(labels, label_indices);
+ print_info("Detecting all edges... [done, ");
+ print_value("%g", tt.toc());
+ print_info(" ms]\n");
// Make gray point clouds
- for (auto &point : cloud->points)
- {
- std::uint8_t gray = std::uint8_t ((point.r + point.g + point.b) / 3);
+ for (auto& point : cloud->points) {
+ std::uint8_t gray = std::uint8_t((point.r + point.g + point.b) / 3);
point.r = point.g = point.b = gray;
}
// Display edges in PCLVisualizer
- viewer.setSize (640, 480);
- viewer.addCoordinateSystem (0.2f, "global");
- viewer.addPointCloud (cloud, "original point cloud");
+ viewer.setSize(640, 480);
+ viewer.addCoordinateSystem(0.2f, "global");
+ viewer.addPointCloud(cloud, "original point cloud");
viewer.registerKeyboardCallback(&keyboard_callback);
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
- occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
- nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
- high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
- rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>);
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges(
+ new pcl::PointCloud<pcl::PointXYZRGBA>),
+ occluded_edges(new pcl::PointCloud<pcl::PointXYZRGBA>),
+ nan_boundary_edges(new pcl::PointCloud<pcl::PointXYZRGBA>),
+ high_curvature_edges(new pcl::PointCloud<pcl::PointXYZRGBA>),
+ rgb_edges(new pcl::PointCloud<pcl::PointXYZRGBA>);
- pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges);
- pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
- pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
- pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
- pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);
+ pcl::copyPointCloud(*cloud, label_indices[0].indices, *nan_boundary_edges);
+ pcl::copyPointCloud(*cloud, label_indices[1].indices, *occluding_edges);
+ pcl::copyPointCloud(*cloud, label_indices[2].indices, *occluded_edges);
+ pcl::copyPointCloud(*cloud, label_indices[3].indices, *high_curvature_edges);
+ pcl::copyPointCloud(*cloud, label_indices[4].indices, *rgb_edges);
const int point_size = 2;
- viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
-
- viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
-
- viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
-
- viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");
-
- viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
- viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
-
- while (!viewer.wasStopped ())
- {
- viewer.spinOnce ();
+ viewer.addPointCloud<pcl::PointXYZRGBA>(nan_boundary_edges, "nan boundary edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
+
+ viewer.addPointCloud<pcl::PointXYZRGBA>(occluding_edges, "occluding edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
+
+ viewer.addPointCloud<pcl::PointXYZRGBA>(occluded_edges, "occluded edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
+
+ viewer.addPointCloud<pcl::PointXYZRGBA>(high_curvature_edges, "high curvature edges");
+ viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
+ point_size,
+ "high curvature edges");
+ viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
+ 1.0f,
+ 1.0f,
+ 0.0f,
+ "high curvature edges");
+
+ viewer.addPointCloud<pcl::PointXYZRGBA>(rgb_edges, "rgb edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
+ viewer.setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
+
+ while (!viewer.wasStopped()) {
+ viewer.spinOnce();
std::this_thread::sleep_for(100us);
}
// Combine point clouds and edge labels
pcl::PCLPointCloud2 output_edges;
- toPCLPointCloud2 (labels, output_edges);
- concatenateFields (*input, output_edges, output);
+ toPCLPointCloud2(labels, output_edges);
+ concatenateFields(*input, output_edges, output);
}
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
- print_info ("Detect 3D edges from organized point cloud data. For more information, use: %s -h\n", argv[0]);
+ print_info("Detect 3D edges from organized point cloud data. For more information, "
+ "use: %s -h\n",
+ argv[0]);
bool help = false;
- parse_argument (argc, argv, "-h", help);
- if (argc < 3 || help)
- {
- printHelp (argc, argv);
- return (-1);
+ parse_argument(argc, argv, "-h", help);
+ if (argc < 3 || help) {
+ printHelp(argc, argv);
+ return -1;
}
// Parse the command line arguments for .pcd files
std::vector<int> p_file_indices;
- p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
- if (p_file_indices.size () != 2)
- {
- print_error ("Need one input PCD file and one output PCD file to continue.\n");
- return (-1);
+ p_file_indices = parse_file_extension_argument(argc, argv, ".pcd");
+ if (p_file_indices.size() != 2) {
+ print_error("Need one input PCD file and one output PCD file to continue.\n");
+ return -1;
}
// Command line parsing
float th_dd = default_th_dd;
int max_search = default_max_search;
- parse_argument (argc, argv, "-th_dd", th_dd);
- parse_argument (argc, argv, "-max_search", max_search);
+ parse_argument(argc, argv, "-th_dd", th_dd);
+ parse_argument(argc, argv, "-max_search", max_search);
- print_info ("th_dd: "); print_value ("%f\n", th_dd);
- print_info ("max_search: "); print_value ("%d\n", max_search);
+ print_info("th_dd: ");
+ print_value("%f\n", th_dd);
+ print_info("max_search: ");
+ print_value("%d\n", max_search);
// Load the first file
- pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
- if (!loadCloud (argv[p_file_indices[0]], *cloud))
- return (-1);
+ pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
+ if (!loadCloud(argv[p_file_indices[0]], *cloud))
+ return -1;
// Perform the feature estimation
pcl::PCLPointCloud2 output;
- compute (cloud, output, th_dd, max_search);
+ compute(cloud, output, th_dd, max_search);
// Save into the second file
- saveCloud (argv[p_file_indices[1]], output);
+ saveCloud(argv[p_file_indices[1]], output);
}
-
* POSSIBILITY OF SUCH DAMAGE.
*
*/
-#include <pcl/io/io.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
-#include <pcl/ModelCoefficients.h>
-#include <pcl/segmentation/planar_region.h>
-#include <pcl/segmentation/organized_multi_plane_segmentation.h>
-#include <pcl/segmentation/organized_connected_component_segmentation.h>
#include <pcl/filters/extract_indices.h>
-#include <pcl/console/parse.h>
#include <pcl/geometry/polygon_operations.h>
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/segmentation/organized_connected_component_segmentation.h>
+#include <pcl/segmentation/organized_multi_plane_segmentation.h>
+#include <pcl/segmentation/planar_region.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/ModelCoefficients.h>
-template<typename PointT>
-class PCDOrganizedMultiPlaneSegmentation
-{
- private:
- pcl::visualization::PCLVisualizer viewer;
- typename pcl::PointCloud<PointT>::ConstPtr cloud;
- bool refine_;
- float threshold_;
- bool depth_dependent_;
- bool polygon_refinement_;
- public:
- PCDOrganizedMultiPlaneSegmentation (typename pcl::PointCloud<PointT>::ConstPtr cloud_, bool refine)
- : viewer ("Viewer")
- , cloud (cloud_)
- , refine_ (refine)
- , threshold_ (0.02f)
- , depth_dependent_ (true)
- , polygon_refinement_ (false)
- {
- viewer.setBackgroundColor (0, 0, 0);
- //viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
- //viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud");
- viewer.addCoordinateSystem (1.0, "global");
- viewer.initCameraParameters ();
- viewer.registerKeyboardCallback(&PCDOrganizedMultiPlaneSegmentation::keyboard_callback, *this, nullptr);
- }
+template <typename PointT>
+class PCDOrganizedMultiPlaneSegmentation {
+private:
+ pcl::visualization::PCLVisualizer viewer;
+ typename pcl::PointCloud<PointT>::ConstPtr cloud;
+ bool refine_;
+ float threshold_;
+ bool depth_dependent_;
+ bool polygon_refinement_;
+
+public:
+ PCDOrganizedMultiPlaneSegmentation(typename pcl::PointCloud<PointT>::ConstPtr cloud_,
+ bool refine)
+ : viewer("Viewer")
+ , cloud(cloud_)
+ , refine_(refine)
+ , threshold_(0.02f)
+ , depth_dependent_(true)
+ , polygon_refinement_(false)
+ {
+ viewer.setBackgroundColor(0, 0, 0);
+ viewer.addCoordinateSystem(1.0, "global");
+ viewer.initCameraParameters();
+ viewer.registerKeyboardCallback(
+ &PCDOrganizedMultiPlaneSegmentation::keyboard_callback, *this, nullptr);
+ }
- void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
- {
- // do stuff and visualize here
- if (event.keyUp ())
- {
- switch (event.getKeyCode ())
- {
- case 'b':
- case 'B':
- if (threshold_ < 0.1f)
- threshold_ += 0.001f;
- process ();
- break;
- case 'v':
- case 'V':
- if (threshold_ > 0.001f)
- threshold_ -= 0.001f;
- process ();
- break;
-
- case 'n':
- case 'N':
- depth_dependent_ = !depth_dependent_;
- process ();
- break;
-
- case 'm':
- case 'M':
- polygon_refinement_ = !polygon_refinement_;
- process ();
- break;
- }
+ void
+ keyboard_callback(const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ // do stuff and visualize here
+ if (event.keyUp()) {
+ switch (event.getKeyCode()) {
+ case 'b':
+ case 'B':
+ if (threshold_ < 0.1f)
+ threshold_ += 0.001f;
+ process();
+ break;
+ case 'v':
+ case 'V':
+ if (threshold_ > 0.001f)
+ threshold_ -= 0.001f;
+ process();
+ break;
+
+ case 'n':
+ case 'N':
+ depth_dependent_ = !depth_dependent_;
+ process();
+ break;
+
+ case 'm':
+ case 'M':
+ polygon_refinement_ = !polygon_refinement_;
+ process();
+ break;
}
}
-
- void
- process ()
- {
- std::cout << "threshold: " << threshold_ << std::endl;
- std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n");
- unsigned char red [6] = {255, 0, 0, 255, 255, 0};
- unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
- unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
-
- pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
- ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
- ne.setMaxDepthChangeFactor (0.02f);
- ne.setNormalSmoothingSize (20.0f);
-
- typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
- refinement_compare->setDistanceThreshold (threshold_, depth_dependent_);
-
- pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
- mps.setMinInliers (5000);
- mps.setAngularThreshold (0.017453 * 3.0); //3 degrees
- mps.setDistanceThreshold (0.03); //2cm
- mps.setRefinementComparator (refinement_compare);
-
- std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
- typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
- typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>);
- char name[1024];
-
- typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
- double normal_start = pcl::getTime ();
- ne.setInputCloud (cloud);
- ne.compute (*normal_cloud);
- double normal_end = pcl::getTime ();
- std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
-
- double plane_extract_start = pcl::getTime ();
- mps.setInputNormals (normal_cloud);
- mps.setInputCloud (cloud);
- if (refine_)
- mps.segmentAndRefine (regions);
- else
- mps.segment (regions);
- double plane_extract_end = pcl::getTime ();
- std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl;
- std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
-
- typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
-
- viewer.removeAllPointClouds (0);
- viewer.removeAllShapes (0);
- pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
- viewer.addPointCloud<PointT> (cloud, single_color, "cloud");
-
- pcl::PlanarPolygon<PointT> approx_polygon;
- //Draw Visualization
- for (std::size_t i = 0; i < regions.size (); i++)
- {
- Eigen::Vector3f centroid = regions[i].getCentroid ();
- Eigen::Vector4f model = regions[i].getCoefficients ();
- pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
- pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
- centroid[1] + (0.5f * model[1]),
- centroid[2] + (0.5f * model[2]));
- sprintf (name, "normal_%d", unsigned (i));
- viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name));
-
- contour->points = regions[i].getContour ();
- sprintf (name, "plane_%02d", int (i));
- pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
- viewer.addPointCloud (contour, color, name);
-
- pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_);
- approx_contour->points = approx_polygon.getContour ();
- std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl;
- typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour;
-
-// sprintf (name, "approx_plane_%02d", int (i));
-// viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
- for (std::size_t idx = 0; idx < approx_contour->points.size (); ++idx)
- {
- sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx));
- viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
- }
+ }
+
+ void
+ process()
+ {
+ std::cout << "threshold: " << threshold_ << std::endl;
+ std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n");
+ unsigned char red[6] = {255, 0, 0, 255, 255, 0};
+ unsigned char grn[6] = {0, 255, 0, 255, 0, 255};
+ unsigned char blu[6] = {0, 0, 255, 0, 255, 255};
+
+ pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+ ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
+ ne.setMaxDepthChangeFactor(0.02f);
+ ne.setNormalSmoothingSize(20.0f);
+
+ typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr
+ refinement_compare(
+ new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>());
+ refinement_compare->setDistanceThreshold(threshold_, depth_dependent_);
+
+ pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
+ mps.setMinInliers(5000);
+ mps.setAngularThreshold(0.017453 * 3.0); // 3 degrees
+ mps.setDistanceThreshold(0.03); // 2cm
+ mps.setRefinementComparator(refinement_compare);
+
+ std::vector<pcl::PlanarRegion<PointT>,
+ Eigen::aligned_allocator<pcl::PlanarRegion<PointT>>>
+ regions;
+ typename pcl::PointCloud<PointT>::Ptr contour(new pcl::PointCloud<PointT>);
+ typename pcl::PointCloud<PointT>::Ptr approx_contour(new pcl::PointCloud<PointT>);
+ char name[1024];
+
+ typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud(
+ new pcl::PointCloud<pcl::Normal>);
+ double normal_start = pcl::getTime();
+ ne.setInputCloud(cloud);
+ ne.compute(*normal_cloud);
+ double normal_end = pcl::getTime();
+ std::cout << "Normal Estimation took " << double(normal_end - normal_start)
+ << std::endl;
+
+ double plane_extract_start = pcl::getTime();
+ mps.setInputNormals(normal_cloud);
+ mps.setInputCloud(cloud);
+ if (refine_)
+ mps.segmentAndRefine(regions);
+ else
+ mps.segment(regions);
+ double plane_extract_end = pcl::getTime();
+ std::cout << "Plane extraction took "
+ << double(plane_extract_end - plane_extract_start)
+ << " with planar regions found: " << regions.size() << std::endl;
+ std::cout << "Frame took " << double(plane_extract_end - normal_start) << std::endl;
+
+ typename pcl::PointCloud<PointT>::Ptr cluster(new pcl::PointCloud<PointT>);
+
+ viewer.removeAllPointClouds(0);
+ viewer.removeAllShapes(0);
+ pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color(
+ cloud, 0, 255, 0);
+ viewer.addPointCloud<PointT>(cloud, single_color, "cloud");
+
+ pcl::PlanarPolygon<PointT> approx_polygon;
+ // Draw Visualization
+ for (std::size_t i = 0; i < regions.size(); i++) {
+ Eigen::Vector3f centroid = regions[i].getCentroid();
+ Eigen::Vector4f model = regions[i].getCoefficients();
+ pcl::PointXYZ pt1 = pcl::PointXYZ(centroid[0], centroid[1], centroid[2]);
+ pcl::PointXYZ pt2 = pcl::PointXYZ(centroid[0] + (0.5f * model[0]),
+ centroid[1] + (0.5f * model[1]),
+ centroid[2] + (0.5f * model[2]));
+ sprintf(name, "normal_%d", unsigned(i));
+ viewer.addArrow(pt2, pt1, 1.0, 0, 0, std::string(name));
+
+ contour->points = regions[i].getContour();
+ sprintf(name, "plane_%02d", int(i));
+ pcl::visualization::PointCloudColorHandlerCustom<PointT> color(
+ contour, red[i], grn[i], blu[i]);
+ viewer.addPointCloud(contour, color, name);
+
+ pcl::approximatePolygon(
+ regions[i], approx_polygon, threshold_, polygon_refinement_);
+ approx_contour->points = approx_polygon.getContour();
+ std::cout << "polygon: " << contour->size() << " -> " << approx_contour->size()
+ << std::endl;
+ typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour;
+
+ for (std::size_t idx = 0; idx < approx_contour->points.size(); ++idx) {
+ sprintf(name, "approx_plane_%02d_%03d", int(i), int(idx));
+ viewer.addLine(
+ approx_contour->points[idx],
+ approx_contour->points[(idx + 1) % approx_contour->points.size()],
+ 0.5 * red[i],
+ 0.5 * grn[i],
+ 0.5 * blu[i],
+ name);
}
}
-
- void
- run ()
- {
- // initial processing
- process ();
-
- while (!viewer.wasStopped ())
- viewer.spinOnce (100);
- }
+ }
+
+ void
+ run()
+ {
+ // initial processing
+ process();
+
+ while (!viewer.wasStopped())
+ viewer.spinOnce(100);
+ }
};
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
- bool refine = pcl::console::find_switch (argc, argv, "-refine");
-
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz
(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::io::loadPCDFile (argv[1], *cloud_xyz);
- PCDOrganizedMultiPlaneSegmentation<pcl::PointXYZ> multi_plane_app (cloud_xyz, refine);
- multi_plane_app.run ();
+ bool refine = pcl::console::find_switch(argc, argv, "-refine");
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::io::loadPCDFile(argv[1], *cloud_xyz);
+ PCDOrganizedMultiPlaneSegmentation<pcl::PointXYZ> multi_plane_app(cloud_xyz, refine);
+ multi_plane_app.run();
return 0;
}
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
*
*/
-#include <thread>
-
-#include <pcl/common/common.h>
-#include <pcl/console/time.h>
#include <pcl/common/angles.h>
-#include <pcl/console/print.h>
+#include <pcl/common/common.h>
#include <pcl/console/parse.h>
-
-#include <pcl/io/pcd_io.h>
-
+#include <pcl/console/print.h>
+#include <pcl/console/time.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
-
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
-
-#include <pcl/segmentation/organized_multi_plane_segmentation.h>
+#include <pcl/geometry/polygon_operations.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/segmentation/edge_aware_plane_comparator.h>
#include <pcl/segmentation/euclidean_cluster_comparator.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/segmentation/organized_connected_component_segmentation.h>
-#include <pcl/segmentation/edge_aware_plane_comparator.h>
+#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/segmentation/extract_polygonal_prism_data.h>
-#include <pcl/segmentation/extract_clusters.h>
-
#include <pcl/surface/convex_hull.h>
-
-#include <pcl/geometry/polygon_operations.h>
-
-#include <pcl/visualization/point_cloud_handlers.h>
-#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/point_cloud_handlers.h>
+
+#include <thread>
using namespace pcl;
using namespace pcl::console;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
-class ObjectSelection
-{
- public:
- ObjectSelection ()
- : plane_comparator_ (new EdgeAwarePlaneComparator<PointT, Normal>)
- , rgb_data_ ()
- {
- // Set the parameters for planar segmentation
- plane_comparator_->setDistanceThreshold (0.01f, false);
- }
+class ObjectSelection {
+public:
+ ObjectSelection()
+ : plane_comparator_(new EdgeAwarePlaneComparator<PointT, Normal>), rgb_data_()
+ {
+ // Set the parameters for planar segmentation
+ plane_comparator_->setDistanceThreshold(0.01f, false);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ virtual ~ObjectSelection() { delete[] rgb_data_; }
- /////////////////////////////////////////////////////////////////////////
- virtual ~ObjectSelection ()
- {
- if (rgb_data_)
- delete[] rgb_data_;
+ /////////////////////////////////////////////////////////////////////////
+ void
+ estimateNormals(const typename PointCloud<PointT>::ConstPtr& input,
+ PointCloud<Normal>& normals)
+ {
+ if (input->isOrganized()) {
+ IntegralImageNormalEstimation<PointT, Normal> ne;
+ // Set the parameters for normal estimation
+ ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
+ ne.setMaxDepthChangeFactor(0.02f);
+ ne.setNormalSmoothingSize(20.0f);
+ // Estimate normals in the cloud
+ ne.setInputCloud(input);
+ ne.compute(normals);
+
+ // Save the distance map for the plane comparator
+ float* map = ne.getDistanceMap(); // This will be deallocated with the
+ // IntegralImageNormalEstimation object...
+ distance_map_.assign(map, map + input->size()); //...so we must copy the data out
+ plane_comparator_->setDistanceMap(distance_map_.data());
+ }
+ else {
+ NormalEstimation<PointT, Normal> ne;
+ ne.setInputCloud(input);
+ ne.setRadiusSearch(0.02f);
+ ne.setSearchMethod(search_);
+ ne.compute(normals);
}
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- estimateNormals (const typename PointCloud<PointT>::ConstPtr &input, PointCloud<Normal> &normals)
- {
- if (input->isOrganized ())
- {
- IntegralImageNormalEstimation<PointT, Normal> ne;
- // Set the parameters for normal estimation
- ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
- ne.setMaxDepthChangeFactor (0.02f);
- ne.setNormalSmoothingSize (20.0f);
- // Estimate normals in the cloud
- ne.setInputCloud (input);
- ne.compute (normals);
-
- // Save the distance map for the plane comparator
- float *map=ne.getDistanceMap ();// This will be deallocated with the IntegralImageNormalEstimation object...
- distance_map_.assign(map, map+input->size() ); //...so we must copy the data out
- plane_comparator_->setDistanceMap(distance_map_.data());
- }
- else
- {
- NormalEstimation<PointT, Normal> ne;
- ne.setInputCloud (input);
- ne.setRadiusSearch (0.02f);
- ne.setSearchMethod (search_);
- ne.compute (normals);
- }
+ /////////////////////////////////////////////////////////////////////////
+ void
+ keyboard_callback(const visualization::KeyboardEvent&, void*)
+ {
+ // if (event.getKeyCode())
+ // std::cout << "the key \'" << event.getKeyCode() << "\' (" << event.getKeyCode()
+ // << ") was";
+ // else
+ // std::cout << "the special key \'" << event.getKeySym() << "\' was";
+ // if (event.keyDown())
+ // std::cout << " pressed" << std::endl;
+ // else
+ // std::cout << " released" << std::endl;
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ mouse_callback(const visualization::MouseEvent& mouse_event, void*)
+ {
+ if (mouse_event.getType() == visualization::MouseEvent::MouseButtonPress &&
+ mouse_event.getButton() == visualization::MouseEvent::LeftButton) {
+ std::cout << "left button pressed @ " << mouse_event.getX() << " , "
+ << mouse_event.getY() << std::endl;
}
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- keyboard_callback (const visualization::KeyboardEvent&, void*)
- {
- //if (event.getKeyCode())
- // std::cout << "the key \'" << event.getKeyCode() << "\' (" << event.getKeyCode() << ") was";
- //else
- // std::cout << "the special key \'" << event.getKeySym() << "\' was";
- //if (event.keyDown())
- // std::cout << " pressed" << std::endl;
- //else
- // std::cout << " released" << std::endl;
+ /////////////////////////////////////////////////////////////////////////
+ /** \brief
+ * Given a plane, and the set of inlier indices representing it,
+ * segment out the object of intererest supported by it.
+ *
+ * \param[in] picked_idx the index of a point on the object
+ * \param[in] cloud the full point cloud dataset
+ * \param[in] plane_indices a set of indices representing the plane supporting the
+ * object of interest
+ * \param[out] object the segmented resultant object
+ * \param[out] object the segmented resultant object
+ */
+ void
+ segmentObject(int picked_idx,
+ const typename PointCloud<PointT>::ConstPtr& cloud,
+ const PointIndices::Ptr& plane_indices,
+ PointCloud<PointT>& object)
+ {
+ typename PointCloud<PointT>::Ptr plane_hull(new PointCloud<PointT>);
+
+ // Compute the convex hull of the plane
+ ConvexHull<PointT> chull;
+ chull.setDimension(2);
+ chull.setInputCloud(cloud);
+ chull.setIndices(plane_indices);
+ chull.reconstruct(*plane_hull);
+
+ // Remove the plane indices from the data
+ typename PointCloud<PointT>::Ptr plane(new PointCloud<PointT>);
+ ExtractIndices<PointT> extract(true);
+ extract.setInputCloud(cloud);
+ extract.setIndices(plane_indices);
+ extract.setNegative(false);
+ extract.filter(*plane);
+ PointIndices::Ptr indices_but_the_plane(new PointIndices);
+ extract.getRemovedIndices(*indices_but_the_plane);
+
+ // Extract all clusters above the hull
+ PointIndices::Ptr points_above_plane(new PointIndices);
+ ExtractPolygonalPrismData<PointT> exppd;
+ exppd.setInputCloud(cloud);
+ exppd.setIndices(indices_but_the_plane);
+ exppd.setInputPlanarHull(plane_hull);
+ exppd.setViewPoint(cloud->points[picked_idx].x,
+ cloud->points[picked_idx].y,
+ cloud->points[picked_idx].z);
+ exppd.setHeightLimits(0.001, 0.5); // up to half a meter
+ exppd.segment(*points_above_plane);
+
+ std::vector<PointIndices> euclidean_label_indices;
+ // Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
+ if (cloud_->isOrganized()) {
+ // Use an organized clustering segmentation to extract the individual clusters
+ typename EuclideanClusterComparator<PointT, Label>::Ptr
+ euclidean_cluster_comparator(new EuclideanClusterComparator<PointT, Label>);
+ euclidean_cluster_comparator->setInputCloud(cloud);
+ euclidean_cluster_comparator->setDistanceThreshold(0.03f, false);
+ // Set the entire scene to false, and the inliers of the objects located on top of
+ // the plane to true
+ Label l;
+ l.label = 0;
+ PointCloud<Label>::Ptr scene(
+ new PointCloud<Label>(cloud->width, cloud->height, l));
+ // Mask the objects that we want to split into clusters
+ for (const int& index : points_above_plane->indices)
+ scene->points[index].label = 1;
+ euclidean_cluster_comparator->setLabels(scene);
+
+ typename EuclideanClusterComparator<PointT, Label>::ExcludeLabelSetPtr
+ exclude_labels(
+ new typename EuclideanClusterComparator<PointT, Label>::ExcludeLabelSet);
+ exclude_labels->insert(0);
+ euclidean_cluster_comparator->setExcludeLabels(exclude_labels);
+
+ OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation(
+ euclidean_cluster_comparator);
+ euclidean_segmentation.setInputCloud(cloud);
+
+ PointCloud<Label> euclidean_labels;
+ euclidean_segmentation.segment(euclidean_labels, euclidean_label_indices);
}
-
- /////////////////////////////////////////////////////////////////////////
- void
- mouse_callback (const visualization::MouseEvent& mouse_event, void*)
- {
- if (mouse_event.getType() == visualization::MouseEvent::MouseButtonPress && mouse_event.getButton() == visualization::MouseEvent::LeftButton)
- {
- std::cout << "left button pressed @ " << mouse_event.getX () << " , " << mouse_event.getY () << std::endl;
- }
+ else {
+ print_highlight(
+ stderr,
+ "Extracting individual clusters from the points above the reference plane ");
+ TicToc tt;
+ tt.tic();
+
+ EuclideanClusterExtraction<PointT> ec;
+ ec.setClusterTolerance(0.02); // 2cm
+ ec.setMinClusterSize(100);
+ ec.setSearchMethod(search_);
+ ec.setInputCloud(cloud);
+ ec.setIndices(points_above_plane);
+ ec.extract(euclidean_label_indices);
+
+ print_info("[done, ");
+ print_value("%g", tt.toc());
+ print_info(" ms : ");
+ print_value("%lu", euclidean_label_indices.size());
+ print_info(" clusters]\n");
}
- /////////////////////////////////////////////////////////////////////////
- /** \brief Given a plane, and the set of inlier indices representing it,
- * segment out the object of intererest supported by it.
- *
- * \param[in] picked_idx the index of a point on the object
- * \param[in] cloud the full point cloud dataset
- * \param[in] plane_indices a set of indices representing the plane supporting the object of interest
- * \param[out] object the segmented resultant object
- */
- void
- segmentObject (int picked_idx,
- const typename PointCloud<PointT>::ConstPtr &cloud,
- const PointIndices::Ptr &plane_indices,
- PointCloud<PointT> &object)
- {
- typename PointCloud<PointT>::Ptr plane_hull (new PointCloud<PointT>);
-
- // Compute the convex hull of the plane
- ConvexHull<PointT> chull;
- chull.setDimension (2);
- chull.setInputCloud (cloud);
- chull.setIndices (plane_indices);
- chull.reconstruct (*plane_hull);
-
- // Remove the plane indices from the data
- typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
- ExtractIndices<PointT> extract (true);
- extract.setInputCloud (cloud);
- extract.setIndices (plane_indices);
- extract.setNegative (false);
- extract.filter (*plane);
- PointIndices::Ptr indices_but_the_plane (new PointIndices);
- extract.getRemovedIndices (*indices_but_the_plane);
-
- // Extract all clusters above the hull
- PointIndices::Ptr points_above_plane (new PointIndices);
- ExtractPolygonalPrismData<PointT> exppd;
- exppd.setInputCloud (cloud);
- exppd.setIndices (indices_but_the_plane);
- exppd.setInputPlanarHull (plane_hull);
- exppd.setViewPoint (cloud->points[picked_idx].x, cloud->points[picked_idx].y, cloud->points[picked_idx].z);
- exppd.setHeightLimits (0.001, 0.5); // up to half a meter
- exppd.segment (*points_above_plane);
-
- std::vector<PointIndices> euclidean_label_indices;
- // Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
- if (cloud_->isOrganized ())
- {
- // Use an organized clustering segmentation to extract the individual clusters
- typename EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
- euclidean_cluster_comparator->setInputCloud (cloud);
- euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
- // Set the entire scene to false, and the inliers of the objects located on top of the plane to true
- Label l; l.label = 0;
- PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
- // Mask the objects that we want to split into clusters
- for (const int &index : points_above_plane->indices)
- scene->points[index].label = 1;
- euclidean_cluster_comparator->setLabels (scene);
-
- typename EuclideanClusterComparator<PointT, Label>::ExcludeLabelSetPtr exclude_labels (new typename EuclideanClusterComparator<PointT, Label>::ExcludeLabelSet);
- exclude_labels->insert (0);
- euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
-
- OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
- euclidean_segmentation.setInputCloud (cloud);
-
- PointCloud<Label> euclidean_labels;
- euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
- }
- else
- {
- print_highlight (stderr, "Extracting individual clusters from the points above the reference plane ");
- TicToc tt; tt.tic ();
-
- EuclideanClusterExtraction<PointT> ec;
- ec.setClusterTolerance (0.02); // 2cm
- ec.setMinClusterSize (100);
- ec.setSearchMethod (search_);
- ec.setInputCloud (cloud);
- ec.setIndices (points_above_plane);
- ec.extract (euclidean_label_indices);
-
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", euclidean_label_indices.size ()); print_info (" clusters]\n");
- }
-
- // For each cluster found
- bool cluster_found = false;
- for (const auto &euclidean_label_index : euclidean_label_indices)
- {
- if (cluster_found)
- break;
- // Check if the point that we picked belongs to it
- for (std::size_t j = 0; j < euclidean_label_index.indices.size (); ++j)
- {
- if (picked_idx != euclidean_label_index.indices[j])
- continue;
- copyPointCloud (*cloud, euclidean_label_index.indices, object);
- cluster_found = true;
- break;
- }
+ // For each cluster found
+ bool cluster_found = false;
+ for (const auto& euclidean_label_index : euclidean_label_indices) {
+ if (cluster_found)
+ break;
+ // Check if the point that we picked belongs to it
+ for (std::size_t j = 0; j < euclidean_label_index.indices.size(); ++j) {
+ if (picked_idx != euclidean_label_index.indices[j])
+ continue;
+ copyPointCloud(*cloud, euclidean_label_index.indices, object);
+ cluster_found = true;
+ break;
}
}
+ }
- /////////////////////////////////////////////////////////////////////////
- void
- segment (const PointT &picked_point,
- int picked_idx,
- PlanarRegion<PointT> ®ion,
- typename PointCloud<PointT>::Ptr &object)
- {
- object.reset ();
-
- // Segment out all planes
- std::vector<ModelCoefficients> model_coefficients;
- std::vector<PointIndices> inlier_indices, boundary_indices;
- std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > > regions;
-
- // Prefer a faster method if the cloud is organized, over RANSAC
- if (cloud_->isOrganized ())
- {
- print_highlight (stderr, "Estimating normals ");
- TicToc tt; tt.tic ();
- // Estimate normals
- PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
- estimateNormals (cloud_, *normal_cloud);
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", normal_cloud->size ()); print_info (" points]\n");
-
- OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
- mps.setMinInliers (1000);
- mps.setAngularThreshold (deg2rad (3.0)); // 3 degrees
- mps.setDistanceThreshold (0.03); // 2 cm
- mps.setMaximumCurvature (0.001); // a small curvature
- mps.setProjectPoints (true);
- mps.setComparator (plane_comparator_);
- mps.setInputNormals (normal_cloud);
- mps.setInputCloud (cloud_);
-
- // Use one of the overloaded segmentAndRefine calls to get all the information that we want out
- PointCloud<Label>::Ptr labels (new PointCloud<Label>);
- std::vector<PointIndices> label_indices;
- mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
- }
- else
- {
- SACSegmentation<PointT> seg;
- seg.setOptimizeCoefficients (true);
- seg.setModelType (SACMODEL_PLANE);
- seg.setMethodType (SAC_RANSAC);
- seg.setMaxIterations (10000);
- seg.setDistanceThreshold (0.005);
-
- // Copy XYZ and Normals to a new cloud
- typename PointCloud<PointT>::Ptr cloud_segmented (new PointCloud<PointT> (*cloud_));
- typename PointCloud<PointT>::Ptr cloud_remaining (new PointCloud<PointT>);
-
- ModelCoefficients coefficients;
- ExtractIndices<PointT> extract;
- PointIndices::Ptr inliers (new PointIndices ());
-
- // Up until 30% of the original cloud is left
- int i = 1;
- while (double (cloud_segmented->size ()) > 0.3 * double (cloud_->size ()))
- {
- seg.setInputCloud (cloud_segmented);
-
- print_highlight (stderr, "Searching for the largest plane (%2.0d) ", i++);
- TicToc tt; tt.tic ();
- seg.segment (*inliers, coefficients);
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", inliers->indices.size ()); print_info (" points]\n");
-
- // No datasets could be found anymore
- if (inliers->indices.empty ())
- break;
-
- // Save this plane
- PlanarRegion<PointT> region;
- region.setCoefficients (coefficients);
- regions.push_back (region);
-
- inlier_indices.push_back (*inliers);
- model_coefficients.push_back (coefficients);
-
- // Extract the outliers
- extract.setInputCloud (cloud_segmented);
- extract.setIndices (inliers);
- extract.setNegative (true);
- extract.filter (*cloud_remaining);
- cloud_segmented.swap (cloud_remaining);
- }
- }
- print_highlight ("Number of planar regions detected: %lu for a cloud of %lu points\n", regions.size (), cloud_->size ());
-
- double max_dist = std::numeric_limits<double>::max ();
- // Compute the distances from all the planar regions to the picked point, and select the closest region
- int idx = -1;
- for (std::size_t i = 0; i < regions.size (); ++i)
- {
- double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ());
- if (dist < max_dist)
- {
- max_dist = dist;
- idx = static_cast<int> (i);
- }
- }
+ /////////////////////////////////////////////////////////////////////////
+ void
+ segment(const PointT& picked_point,
+ int picked_idx,
+ PlanarRegion<PointT>& region,
+ typename PointCloud<PointT>::Ptr& object)
+ {
+ object.reset();
+
+ // Segment out all planes
+ std::vector<ModelCoefficients> model_coefficients;
+ std::vector<PointIndices> inlier_indices, boundary_indices;
+ std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT>>>
+ regions;
+
+ // Prefer a faster method if the cloud is organized, over RANSAC
+ if (cloud_->isOrganized()) {
+ print_highlight(stderr, "Estimating normals ");
+ TicToc tt;
+ tt.tic();
+ // Estimate normals
+ PointCloud<Normal>::Ptr normal_cloud(new PointCloud<Normal>);
+ estimateNormals(cloud_, *normal_cloud);
+ print_info("[done, ");
+ print_value("%g", tt.toc());
+ print_info(" ms : ");
+ print_value("%lu", normal_cloud->size());
+ print_info(" points]\n");
+
+ OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
+ mps.setMinInliers(1000);
+ mps.setAngularThreshold(deg2rad(3.0)); // 3 degrees
+ mps.setDistanceThreshold(0.03); // 2 cm
+ mps.setMaximumCurvature(0.001); // a small curvature
+ mps.setProjectPoints(true);
+ mps.setComparator(plane_comparator_);
+ mps.setInputNormals(normal_cloud);
+ mps.setInputCloud(cloud_);
+
+ // Use one of the overloaded segmentAndRefine calls to get all the information
+ // that we want out
+ PointCloud<Label>::Ptr labels(new PointCloud<Label>);
+ std::vector<PointIndices> label_indices;
+ mps.segmentAndRefine(regions,
+ model_coefficients,
+ inlier_indices,
+ labels,
+ label_indices,
+ boundary_indices);
+ }
+ else {
+ SACSegmentation<PointT> seg;
+ seg.setOptimizeCoefficients(true);
+ seg.setModelType(SACMODEL_PLANE);
+ seg.setMethodType(SAC_RANSAC);
+ seg.setMaxIterations(10000);
+ seg.setDistanceThreshold(0.005);
+
+ // Copy XYZ and Normals to a new cloud
+ typename PointCloud<PointT>::Ptr cloud_segmented(new PointCloud<PointT>(*cloud_));
+ typename PointCloud<PointT>::Ptr cloud_remaining(new PointCloud<PointT>);
+
+ ModelCoefficients coefficients;
+ ExtractIndices<PointT> extract;
+ PointIndices::Ptr inliers(new PointIndices());
+
+ // Up until 30% of the original cloud is left
+ int i = 1;
+ while (double(cloud_segmented->size()) > 0.3 * double(cloud_->size())) {
+ seg.setInputCloud(cloud_segmented);
+
+ print_highlight(stderr, "Searching for the largest plane (%2.0d) ", i++);
+ TicToc tt;
+ tt.tic();
+ seg.segment(*inliers, coefficients);
+ print_info("[done, ");
+ print_value("%g", tt.toc());
+ print_info(" ms : ");
+ print_value("%lu", inliers->indices.size());
+ print_info(" points]\n");
+
+ // No datasets could be found anymore
+ if (inliers->indices.empty())
+ break;
- // Get the plane that holds the object of interest
- if (idx != -1)
- {
- plane_indices_.reset (new PointIndices (inlier_indices[idx]));
+ // Save this plane
+ PlanarRegion<PointT> region;
+ region.setCoefficients(coefficients);
+ regions.push_back(region);
- if (cloud_->isOrganized ())
- {
- approximatePolygon (regions[idx], region, 0.01f, false, true);
- print_highlight ("Planar region: %lu points initial, %lu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ());
- }
- else
- {
- // Save the current region
- region = regions[idx];
-
- print_highlight (stderr, "Obtaining the boundary points for the region ");
- TicToc tt; tt.tic ();
- // Project the inliers to obtain a better hull
- typename PointCloud<PointT>::Ptr cloud_projected (new PointCloud<PointT>);
- ModelCoefficients::Ptr coefficients (new ModelCoefficients (model_coefficients[idx]));
- ProjectInliers<PointT> proj;
- proj.setModelType (SACMODEL_PLANE);
- proj.setInputCloud (cloud_);
- proj.setIndices (plane_indices_);
- proj.setModelCoefficients (coefficients);
- proj.filter (*cloud_projected);
-
- // Compute the boundary points as a ConvexHull
- ConvexHull<PointT> chull;
- chull.setDimension (2);
- chull.setInputCloud (cloud_projected);
- PointCloud<PointT> plane_hull;
- chull.reconstruct (plane_hull);
- region.setContour (plane_hull);
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", plane_hull.size ()); print_info (" points]\n");
- }
+ inlier_indices.push_back(*inliers);
+ model_coefficients.push_back(coefficients);
+ // Extract the outliers
+ extract.setInputCloud(cloud_segmented);
+ extract.setIndices(inliers);
+ extract.setNegative(true);
+ extract.filter(*cloud_remaining);
+ cloud_segmented.swap(cloud_remaining);
}
-
- // Segment the object of interest
- if (plane_indices_ && !plane_indices_->indices.empty ())
- {
- plane_.reset (new PointCloud<PointT>);
- copyPointCloud (*cloud_, plane_indices_->indices, *plane_);
- object.reset (new PointCloud<PointT>);
- segmentObject (picked_idx, cloud_, plane_indices_, *object);
+ }
+ print_highlight(
+ "Number of planar regions detected: %lu for a cloud of %lu points\n",
+ regions.size(),
+ cloud_->size());
+
+ double max_dist = std::numeric_limits<double>::max();
+ // Compute the distances from all the planar regions to the picked point, and select
+ // the closest region
+ int idx = -1;
+ for (std::size_t i = 0; i < regions.size(); ++i) {
+ double dist = pointToPlaneDistance(picked_point, regions[i].getCoefficients());
+ if (dist < max_dist) {
+ max_dist = dist;
+ idx = static_cast<int>(i);
}
}
- /////////////////////////////////////////////////////////////////////////
- /** \brief Point picking callback. This gets called when the user selects
- * a 3D point on screen (in the PCLVisualizer window) using Shift+click.
- *
- * \param[in] event the event that triggered the call
- */
- void
- pp_callback (const visualization::PointPickingEvent& event, void*)
- {
- // Check to see if we got a valid point. Early exit.
- int idx = event.getPointIndex ();
- if (idx == -1)
- return;
-
- std::vector<int> indices (1);
- std::vector<float> distances (1);
-
- // Get the point that was picked
- PointT picked_pt;
- event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
-
- print_info (stderr, "Picked point with index %d, and coordinates %f, %f, %f.\n", idx, picked_pt.x, picked_pt.y, picked_pt.z);
-
- // Add a sphere to it in the PCLVisualizer window
- stringstream ss;
- ss << "sphere_" << idx;
- cloud_viewer_->addSphere (picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str ());
-
- // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
- search_->nearestKSearch (picked_pt, 1, indices, distances);
-
- // Add some marker to the image
- if (image_viewer_)
- {
- // Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is bottom left.
- std::uint32_t width = search_->getInputCloud ()->width,
- height = search_->getInputCloud ()->height;
- int v = height - indices[0] / width,
- u = indices[0] % width;
-
- image_viewer_->addCircle (u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
- image_viewer_->addFilledRectangle (u-5, u+5, v-5, v+5, 0.0, 1.0, 0.0, "boxes", 0.5);
- image_viewer_->markPoint (u, v, visualization::red_color, visualization::blue_color, 10);
- }
+ // Get the plane that holds the object of interest
+ if (idx != -1) {
+ plane_indices_.reset(new PointIndices(inlier_indices[idx]));
- // Segment the region that we're interested in, by employing a two step process:
- // * first, segment all the planes in the scene, and find the one closest to our picked point
- // * then, use euclidean clustering to find the object that we clicked on and return it
- PlanarRegion<PointT> region;
- segment (picked_pt, indices[0], region, object_);
-
- // If no region could be determined, exit
- if (region.getContour ().empty ())
- {
- PCL_ERROR ("No planar region detected. Please select another point or relax the thresholds and continue.\n");
- return;
+ if (cloud_->isOrganized()) {
+ approximatePolygon(regions[idx], region, 0.01f, false, true);
+ print_highlight(
+ "Planar region: %lu points initial, %lu points after refinement.\n",
+ regions[idx].getContour().size(),
+ region.getContour().size());
}
- // Else, draw it on screen
- cloud_viewer_->addPolygon (region, 0.0, 0.0, 1.0, "region");
- cloud_viewer_->setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");
-
- // Draw in image space
- if (image_viewer_)
- {
- image_viewer_->addPlanarPolygon (search_->getInputCloud (), region, 0.0, 0.0, 1.0, "refined_region", 1.0);
+ else {
+ // Save the current region
+ region = regions[idx];
+
+ print_highlight(stderr, "Obtaining the boundary points for the region ");
+ TicToc tt;
+ tt.tic();
+ // Project the inliers to obtain a better hull
+ typename PointCloud<PointT>::Ptr cloud_projected(new PointCloud<PointT>);
+ ModelCoefficients::Ptr coefficients(
+ new ModelCoefficients(model_coefficients[idx]));
+ ProjectInliers<PointT> proj;
+ proj.setModelType(SACMODEL_PLANE);
+ proj.setInputCloud(cloud_);
+ proj.setIndices(plane_indices_);
+ proj.setModelCoefficients(coefficients);
+ proj.filter(*cloud_projected);
+
+ // Compute the boundary points as a ConvexHull
+ ConvexHull<PointT> chull;
+ chull.setDimension(2);
+ chull.setInputCloud(cloud_projected);
+ PointCloud<PointT> plane_hull;
+ chull.reconstruct(plane_hull);
+ region.setContour(plane_hull);
+ print_info("[done, ");
+ print_value("%g", tt.toc());
+ print_info(" ms : ");
+ print_value("%lu", plane_hull.size());
+ print_info(" points]\n");
}
+ }
- // If no object could be determined, exit
- if (!object_)
- {
- PCL_ERROR ("No object detected. Please select another point or relax the thresholds and continue.\n");
- return;
- }
- // Visualize the object in 3D...
- visualization::PointCloudColorHandlerCustom<PointT> red (object_, 255, 0, 0);
- if (!cloud_viewer_->updatePointCloud (object_, red, "object"))
- cloud_viewer_->addPointCloud (object_, red, "object");
- // ...and 2D
- if (image_viewer_)
- {
- image_viewer_->removeLayer ("object");
- image_viewer_->addMask (search_->getInputCloud (), *object_, "object");
- }
+ // Segment the object of interest
+ if (plane_indices_ && !plane_indices_->indices.empty()) {
+ plane_.reset(new PointCloud<PointT>);
+ copyPointCloud(*cloud_, plane_indices_->indices, *plane_);
+ object.reset(new PointCloud<PointT>);
+ segmentObject(picked_idx, cloud_, plane_indices_, *object);
+ }
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ /**
+ * \brief Point picking callback. This gets called when the user selects
+ * a 3D point on screen (in the PCLVisualizer window) using Shift+click.
+ *
+ * \param[in] event the event that triggered the call
+ */
+ void
+ pp_callback(const visualization::PointPickingEvent& event, void*)
+ {
+ // Check to see if we got a valid point. Early exit.
+ int idx = event.getPointIndex();
+ if (idx == -1)
+ return;
+
+ std::vector<int> indices(1);
+ std::vector<float> distances(1);
+
+ // Get the point that was picked
+ PointT picked_pt;
+ event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z);
+
+ print_info(stderr,
+ "Picked point with index %d, and coordinates %f, %f, %f.\n",
+ idx,
+ picked_pt.x,
+ picked_pt.y,
+ picked_pt.z);
+
+ // Add a sphere to it in the PCLVisualizer window
+ stringstream ss;
+ ss << "sphere_" << idx;
+ cloud_viewer_->addSphere(picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str());
+
+ // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we
+ // must search for the real point
+ search_->nearestKSearch(picked_pt, 1, indices, distances);
+
+ // Add some marker to the image
+ if (image_viewer_) {
+ // Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is
+ // bottom left.
+ std::uint32_t width = search_->getInputCloud()->width,
+ height = search_->getInputCloud()->height;
+ int v = height - indices[0] / width, u = indices[0] % width;
+
+ image_viewer_->addCircle(u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
+ image_viewer_->addFilledRectangle(
+ u - 5, u + 5, v - 5, v + 5, 0.0, 1.0, 0.0, "boxes", 0.5);
+ image_viewer_->markPoint(
+ u, v, visualization::red_color, visualization::blue_color, 10);
+ }
- // ...and 2D
- if (image_viewer_)
- image_viewer_->addRectangle (search_->getInputCloud (), *object_);
+ // Segment the region that we're interested in, by employing a two step process:
+ // * first, segment all the planes in the scene, and find the one closest to our
+ // picked point
+ // * then, use euclidean clustering to find the object that we clicked on and
+ // return it
+ PlanarRegion<PointT> region;
+ segment(picked_pt, indices[0], region, object_);
+
+ // If no region could be determined, exit
+ if (region.getContour().empty()) {
+ PCL_ERROR("No planar region detected. Please select another point or relax the "
+ "thresholds and continue.\n");
+ return;
}
-
- /////////////////////////////////////////////////////////////////////////
- void
- compute ()
- {
- // Visualize the data
- while (!cloud_viewer_->wasStopped ())
- {
- /*// Add the plane that we're tracking to the cloud visualizer
- PointCloud<PointT>::Ptr plane (new Cloud);
- if (plane_)
- *plane = *plane_;
- visualization::PointCloudColorHandlerCustom<PointT> blue (plane, 0, 255, 0);
- if (!cloud_viewer_->updatePointCloud (plane, blue, "plane"))
- cloud_viewer_->addPointCloud (plane, "plane");
+ // Else, draw it on screen
+ cloud_viewer_->addPolygon(region, 0.0, 0.0, 1.0, "region");
+ cloud_viewer_->setShapeRenderingProperties(
+ visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");
+
+ // Draw in image space
+ if (image_viewer_) {
+ image_viewer_->addPlanarPolygon(
+ search_->getInputCloud(), region, 0.0, 0.0, 1.0, "refined_region", 1.0);
+ }
+
+ // If no object could be determined, exit
+ if (!object_) {
+ PCL_ERROR("No object detected. Please select another point or relax the "
+ "thresholds and continue.\n");
+ return;
+ }
+ // Visualize the object in 3D...
+ visualization::PointCloudColorHandlerCustom<PointT> red(object_, 255, 0, 0);
+ if (!cloud_viewer_->updatePointCloud(object_, red, "object"))
+ cloud_viewer_->addPointCloud(object_, red, "object");
+ // ...and 2D
+ if (image_viewer_) {
+ image_viewer_->removeLayer("object");
+ image_viewer_->addMask(search_->getInputCloud(), *object_, "object");
+ }
+
+ // ...and 2D
+ if (image_viewer_)
+ image_viewer_->addRectangle(search_->getInputCloud(), *object_);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ compute()
+ {
+ // Visualize the data
+ while (!cloud_viewer_->wasStopped()) {
+ /*// Add the plane that we're tracking to the cloud visualizer
+ PointCloud<PointT>::Ptr plane (new Cloud);
+ if (plane_)
+ *plane = *plane_;
+ visualization::PointCloudColorHandlerCustom<PointT> blue (plane, 0, 255, 0);
+ if (!cloud_viewer_->updatePointCloud (plane, blue, "plane"))
+ cloud_viewer_->addPointCloud (plane, "plane");
*/
- cloud_viewer_->spinOnce ();
- if (image_viewer_)
- {
- image_viewer_->spinOnce ();
- if (image_viewer_->wasStopped ())
- break;
- }
- std::this_thread::sleep_for(100us);
+ cloud_viewer_->spinOnce();
+ if (image_viewer_) {
+ image_viewer_->spinOnce();
+ if (image_viewer_->wasStopped())
+ break;
}
+ std::this_thread::sleep_for(100us);
}
-
- /////////////////////////////////////////////////////////////////////////
- void
- initGUI ()
- {
- cloud_viewer_.reset (new visualization::PCLVisualizer ("PointCloud"));
-
- if (cloud_->isOrganized ())
- {
- // If the dataset is organized, and has RGB data, create an image viewer
- std::vector<pcl::PCLPointField> fields;
- int rgba_index = -1;
- rgba_index = getFieldIndex<PointT> ("rgba", fields);
-
- if (rgba_index >= 0)
- {
- image_viewer_.reset (new visualization::ImageViewer ("RGB PCLImage"));
-
- image_viewer_->registerMouseCallback (&ObjectSelection::mouse_callback, *this);
- image_viewer_->registerKeyboardCallback(&ObjectSelection::keyboard_callback, *this);
- image_viewer_->setPosition (cloud_->width, 0);
- image_viewer_->setSize (cloud_->width, cloud_->height);
-
- int poff = fields[rgba_index].offset;
- // BGR to RGB
- rgb_data_ = new unsigned char [cloud_->width * cloud_->height * 3];
- for (std::uint32_t i = 0; i < cloud_->width * cloud_->height; ++i)
- {
- RGB rgb;
- memcpy (&rgb, reinterpret_cast<unsigned char*> (&cloud_->points[i]) + poff, sizeof (rgb));
-
- rgb_data_[i * 3 + 0] = rgb.r;
- rgb_data_[i * 3 + 1] = rgb.g;
- rgb_data_[i * 3 + 2] = rgb.b;
- }
- image_viewer_->showRGBImage (rgb_data_, cloud_->width, cloud_->height);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ initGUI()
+ {
+ cloud_viewer_.reset(new visualization::PCLVisualizer("PointCloud"));
+
+ if (cloud_->isOrganized()) {
+ // If the dataset is organized, and has RGB data, create an image viewer
+ std::vector<pcl::PCLPointField> fields;
+ int rgba_index = -1;
+ rgba_index = getFieldIndex<PointT>("rgba", fields);
+
+ if (rgba_index >= 0) {
+ image_viewer_.reset(new visualization::ImageViewer("RGB PCLImage"));
+
+ image_viewer_->registerMouseCallback(&ObjectSelection::mouse_callback, *this);
+ image_viewer_->registerKeyboardCallback(&ObjectSelection::keyboard_callback,
+ *this);
+ image_viewer_->setPosition(cloud_->width, 0);
+ image_viewer_->setSize(cloud_->width, cloud_->height);
+
+ int poff = fields[rgba_index].offset;
+ // BGR to RGB
+ rgb_data_ = new unsigned char[cloud_->width * cloud_->height * 3];
+ for (std::uint32_t i = 0; i < cloud_->width * cloud_->height; ++i) {
+ RGB rgb;
+ memcpy(&rgb,
+ reinterpret_cast<unsigned char*>(&cloud_->points[i]) + poff,
+ sizeof(rgb));
+
+ rgb_data_[i * 3 + 0] = rgb.r;
+ rgb_data_[i * 3 + 1] = rgb.g;
+ rgb_data_[i * 3 + 2] = rgb.b;
}
- cloud_viewer_->setSize (cloud_->width, cloud_->height);
+ image_viewer_->showRGBImage(rgb_data_, cloud_->width, cloud_->height);
}
+ cloud_viewer_->setSize(cloud_->width, cloud_->height);
+ }
- cloud_viewer_->registerMouseCallback (&ObjectSelection::mouse_callback, *this);
- cloud_viewer_->registerKeyboardCallback(&ObjectSelection::keyboard_callback, *this);
- cloud_viewer_->registerPointPickingCallback (&ObjectSelection::pp_callback, *this);
- cloud_viewer_->setPosition (0, 0);
+ cloud_viewer_->registerMouseCallback(&ObjectSelection::mouse_callback, *this);
+ cloud_viewer_->registerKeyboardCallback(&ObjectSelection::keyboard_callback, *this);
+ cloud_viewer_->registerPointPickingCallback(&ObjectSelection::pp_callback, *this);
+ cloud_viewer_->setPosition(0, 0);
+
+ cloud_viewer_->addPointCloud(cloud_, "scene");
+ cloud_viewer_->resetCameraViewpoint("scene");
+ cloud_viewer_->addCoordinateSystem(0.1, 0, 0, 0, "global");
+ }
- cloud_viewer_->addPointCloud (cloud_, "scene");
- cloud_viewer_->resetCameraViewpoint ("scene");
- cloud_viewer_->addCoordinateSystem (0.1, 0, 0, 0, "global");
+ /////////////////////////////////////////////////////////////////////////
+ bool
+ load(const std::string& file)
+ {
+ // Load the input file
+ TicToc tt;
+ tt.tic();
+ print_highlight(stderr, "Loading ");
+ print_value(stderr, "%s ", file.c_str());
+ cloud_.reset(new PointCloud<PointT>);
+ if (io::loadPCDFile(file, *cloud_) < 0) {
+ print_error(stderr, "[error]\n");
+ return false;
}
+ print_info("[done, ");
+ print_value("%g", tt.toc());
+ print_info(" ms : ");
+ print_value("%lu", cloud_->size());
+ print_info(" points]\n");
- /////////////////////////////////////////////////////////////////////////
- bool
- load (const std::string &file)
- {
- // Load the input file
- TicToc tt; tt.tic ();
- print_highlight (stderr, "Loading ");
- print_value (stderr, "%s ", file.c_str ());
- cloud_.reset (new PointCloud<PointT>);
- if (io::loadPCDFile (file, *cloud_) < 0)
- {
- print_error (stderr, "[error]\n");
- return (false);
- }
- print_info ("[done, "); print_value ("%g", tt.toc ());
- print_info (" ms : "); print_value ("%lu", cloud_->size ()); print_info (" points]\n");
-
- if (cloud_->isOrganized ())
- search_.reset (new search::OrganizedNeighbor<PointT>);
- else
- search_.reset (new search::KdTree<PointT>);
+ if (cloud_->isOrganized())
+ search_.reset(new search::OrganizedNeighbor<PointT>);
+ else
+ search_.reset(new search::KdTree<PointT>);
- search_->setInputCloud (cloud_);
+ search_->setInputCloud(cloud_);
- return (true);
- }
-
- /////////////////////////////////////////////////////////////////////////
- void
- save (const std::string &object_file, const std::string &plane_file)
- {
- PCDWriter w;
- if (object_ && !object_->empty ())
- {
- w.writeBinaryCompressed (object_file, *object_);
- w.writeBinaryCompressed (plane_file, *plane_);
- print_highlight ("Object successfully segmented. Saving results in: %s, and %s.\n", object_file.c_str (), plane_file.c_str ());
- }
+ return true;
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ save(const std::string& object_file, const std::string& plane_file)
+ {
+ PCDWriter w;
+ if (object_ && !object_->empty()) {
+ w.writeBinaryCompressed(object_file, *object_);
+ w.writeBinaryCompressed(plane_file, *plane_);
+ print_highlight("Object successfully segmented. Saving results in: %s, and %s.\n",
+ object_file.c_str(),
+ plane_file.c_str());
}
+ }
+
+ visualization::PCLVisualizer::Ptr cloud_viewer_;
+ visualization::ImageViewer::Ptr image_viewer_;
+
+ typename PointCloud<PointT>::Ptr cloud_;
+ typename search::Search<PointT>::Ptr search_;
- visualization::PCLVisualizer::Ptr cloud_viewer_;
- visualization::ImageViewer::Ptr image_viewer_;
-
- typename PointCloud<PointT>::Ptr cloud_;
- typename search::Search<PointT>::Ptr search_;
- private:
- // Segmentation
- typename EdgeAwarePlaneComparator<PointT, Normal>::Ptr plane_comparator_;
- PointIndices::Ptr plane_indices_;
- unsigned char* rgb_data_;
- std::vector<float> distance_map_;
-
- // Results
- typename PointCloud<PointT>::Ptr plane_;
- typename PointCloud<PointT>::Ptr object_;
+private:
+ // Segmentation
+ typename EdgeAwarePlaneComparator<PointT, Normal>::Ptr plane_comparator_;
+ PointIndices::Ptr plane_indices_;
+ unsigned char* rgb_data_;
+ std::vector<float> distance_map_;
+
+ // Results
+ typename PointCloud<PointT>::Ptr plane_;
+ typename PointCloud<PointT>::Ptr object_;
};
/* ---[ */
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
// Parse the command line arguments for .pcd files
std::vector<int> p_file_indices;
- p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
- if (p_file_indices.empty ())
- {
- print_error (" Need at least an input PCD file (e.g. scene.pcd) to continue!\n\n");
- print_info ("Ideally, need an input file, and three output PCD files, e.g., object.pcd, plane.pcd, rest.pcd\n");
- return (-1);
+ p_file_indices = parse_file_extension_argument(argc, argv, ".pcd");
+ if (p_file_indices.empty()) {
+ print_error(" Need at least an input PCD file (e.g. scene.pcd) to continue!\n\n");
+ print_info("Ideally, need an input file, and three output PCD files, e.g., "
+ "object.pcd, plane.pcd, rest.pcd\n");
+ return -1;
}
-
- string object_file = "object.pcd", plane_file = "plane.pcd", rest_file = "rest.pcd";
- if (p_file_indices.size () >= 4)
- rest_file = argv[p_file_indices[3]];
- if (p_file_indices.size () >= 3)
+
+ std::string object_file = "object.pcd";
+ std::string plane_file = "plane.pcd";
+ if (p_file_indices.size() >= 3)
plane_file = argv[p_file_indices[2]];
- if (p_file_indices.size () >= 2)
+ if (p_file_indices.size() >= 2)
object_file = argv[p_file_indices[1]];
-
PCDReader reader;
// Test the header
pcl::PCLPointCloud2 dummy;
- reader.readHeader (argv[p_file_indices[0]], dummy);
- if (dummy.height != 1 && getFieldIndex (dummy, "rgba") != -1)
- {
- print_highlight ("Enabling 2D image viewer mode.\n");
+ reader.readHeader(argv[p_file_indices[0]], dummy);
+ if (dummy.height != 1 && getFieldIndex(dummy, "rgba") != -1) {
+ print_highlight("Enabling 2D image viewer mode.\n");
ObjectSelection<PointXYZRGBA> s;
- if (!s.load (argv[p_file_indices[0]])) return (-1);
- s.initGUI ();
- s.compute ();
- s.save (object_file, plane_file);
+ if (!s.load(argv[p_file_indices[0]]))
+ return -1;
+ s.initGUI();
+ s.compute();
+ s.save(object_file, plane_file);
}
- else
- {
+ else {
ObjectSelection<PointXYZ> s;
- if (!s.load (argv[p_file_indices[0]])) return (-1);
- s.initGUI ();
- s.compute ();
- s.save (object_file, plane_file);
+ if (!s.load(argv[p_file_indices[0]]))
+ return -1;
+ s.initGUI();
+ s.compute();
+ s.save(object_file, plane_file);
}
- return (0);
+ return 0;
}
/* ]--- */
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
*
*/
-//PCL
#include <pcl/apps/pcd_video_player.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
-//STD
-#include <iostream>
-#include <fstream>
-
-//BOOST
-#include <boost/filesystem.hpp>
-#include <boost/foreach.hpp>
-#include <boost/property_tree/xml_parser.hpp>
-#include <boost/property_tree/ptree.hpp>
-
-//QT4
#include <QApplication>
-#include <QMutexLocker>
+#include <QButtonGroup>
#include <QEvent>
-#include <QObject>
#include <QFileDialog>
#include <QGroupBox>
+#include <QMutexLocker>
+#include <QObject>
#include <QRadioButton>
-#include <QButtonGroup>
-// VTK
+#include <vtkCamera.h>
#include <vtkRenderWindow.h>
#include <vtkRendererCollection.h>
-#include <vtkCamera.h>
+
+#include <fstream>
+#include <iostream>
using namespace pcl;
using namespace std;
//////////////////////////////////////////////////////////////////////////////////////////////////////////
-PCDVideoPlayer::PCDVideoPlayer ()
+PCDVideoPlayer::PCDVideoPlayer()
{
cloud_present_ = false;
cloud_modified_ = false;
speed_counter_ = 0;
speed_value_ = 5;
- //Create a timer
- vis_timer_ = new QTimer (this);
- vis_timer_->start (5);//5ms
+ // Create a timer
+ vis_timer_ = new QTimer(this);
+ vis_timer_->start(5); // 5ms
- connect (vis_timer_, SIGNAL (timeout ()), this, SLOT (timeoutSlot ()));
+ connect(vis_timer_, SIGNAL(timeout()), this, SLOT(timeoutSlot()));
ui_ = new Ui::MainWindow;
- ui_->setupUi (this);
-
- this->setWindowTitle ("PCL PCD Video Player");
+ ui_->setupUi(this);
+
+ this->setWindowTitle("PCL PCD Video Player");
// Setup the cloud pointer
- cloud_.reset
(new pcl::PointCloud<pcl::PointXYZRGBA>);
+ cloud_.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Set up the qvtk window
- vis_.reset (new pcl::visualization::PCLVisualizer ("", false));
- ui_->qvtkWidget->SetRenderWindow (vis_->getRenderWindow ());
- vis_->setupInteractor (ui_->qvtkWidget->GetInteractor (), ui_->qvtkWidget->GetRenderWindow ());
- vis_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
- ui_->qvtkWidget->update ();
+ vis_.reset(new pcl::visualization::PCLVisualizer("", false));
+ ui_->qvtkWidget->SetRenderWindow(vis_->getRenderWindow());
+ vis_->setupInteractor(ui_->qvtkWidget->GetInteractor(),
+ ui_->qvtkWidget->GetRenderWindow());
+ vis_->getInteractorStyle()->setKeyboardModifier(
+ pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+ ui_->qvtkWidget->update();
// Connect all buttons
- connect (ui_->playButton, SIGNAL (clicked ()), this, SLOT (playButtonPressed ()));
- connect (ui_->stopButton, SIGNAL (clicked ()), this, SLOT (stopButtonPressed ()));
- connect (ui_->backButton, SIGNAL (clicked ()), this, SLOT (backButtonPressed ()));
- connect (ui_->nextButton, SIGNAL (clicked ()), this, SLOT (nextButtonPressed ()));
-
- connect (ui_->selectFolderButton, SIGNAL (clicked ()), this, SLOT (selectFolderButtonPressed ()));
- connect (ui_->selectFilesButton, SIGNAL (clicked ()), this, SLOT (selectFilesButtonPressed ()));
-
- connect (ui_->indexSlider, SIGNAL (valueChanged (int)), this, SLOT (indexSliderValueChanged (int)));
+ connect(ui_->playButton, SIGNAL(clicked()), this, SLOT(playButtonPressed()));
+ connect(ui_->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonPressed()));
+ connect(ui_->backButton, SIGNAL(clicked()), this, SLOT(backButtonPressed()));
+ connect(ui_->nextButton, SIGNAL(clicked()), this, SLOT(nextButtonPressed()));
+
+ connect(ui_->selectFolderButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(selectFolderButtonPressed()));
+ connect(ui_->selectFilesButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(selectFilesButtonPressed()));
+
+ connect(ui_->indexSlider,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(indexSliderValueChanged(int)));
}
-void
-PCDVideoPlayer::backButtonPressed ()
+void
+PCDVideoPlayer::backButtonPressed()
{
- if(current_frame_ == 0) // Already in the beginning
+ if (current_frame_ == 0) // Already in the beginning
{
- PCL_DEBUG ("[PCDVideoPlayer::nextButtonPressed] : reached the end\n");
+ PCL_DEBUG("[PCDVideoPlayer::nextButtonPressed] : reached the end\n");
current_frame_ = nr_of_frames_ - 1; // reset to end
}
- else
- {
+ else {
current_frame_--;
cloud_modified_ = true;
- ui_->indexSlider->setSliderPosition (current_frame_); // Update the slider position
+ ui_->indexSlider->setSliderPosition(current_frame_); // Update the slider position
}
}
-void
-PCDVideoPlayer::nextButtonPressed ()
+void
+PCDVideoPlayer::nextButtonPressed()
{
if (current_frame_ == (nr_of_frames_ - 1)) // Reached the end
{
- PCL_DEBUG ("[PCDVideoPlayer::nextButtonPressed] : reached the end\n");
+ PCL_DEBUG("[PCDVideoPlayer::nextButtonPressed] : reached the end\n");
current_frame_ = 0; // reset to beginning
}
- else
- {
+ else {
current_frame_++;
cloud_modified_ = true;
- ui_->indexSlider->setSliderPosition (current_frame_); // Update the slider position
+ ui_->indexSlider->setSliderPosition(current_frame_); // Update the slider position
}
}
void
-PCDVideoPlayer::selectFolderButtonPressed ()
+PCDVideoPlayer::selectFolderButtonPressed()
{
- pcd_files_.clear (); // Clear the std::vector
- pcd_paths_.clear (); // Clear the boost filesystem paths
+ pcd_files_.clear(); // Clear the std::vector
+ pcd_paths_.clear(); // Clear the boost filesystem paths
- dir_ = QFileDialog::getExistingDirectory (this,
- tr("Open Directory"),
- "/home",
- QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
+ dir_ = QFileDialog::getExistingDirectory(this,
+ tr("Open Directory"),
+ "/home",
+ QFileDialog::ShowDirsOnly |
+ QFileDialog::DontResolveSymlinks);
boost::filesystem::directory_iterator end_itr;
- if (boost::filesystem::is_directory (dir_.toStdString ()))
- {
- for (boost::filesystem::directory_iterator itr (dir_.toStdString ()); itr != end_itr; ++itr)
- {
- std::string ext = itr->path ().extension ().string ();
- if (ext == ".pcd")
- {
- pcd_files_.push_back (itr->path ().string ());
- pcd_paths_.push_back (itr->path ());
+ if (boost::filesystem::is_directory(dir_.toStdString())) {
+ for (boost::filesystem::directory_iterator itr(dir_.toStdString()); itr != end_itr;
+ ++itr) {
+ std::string ext = itr->path().extension().string();
+ if (ext == ".pcd") {
+ pcd_files_.push_back(itr->path().string());
+ pcd_paths_.push_back(itr->path());
}
- else
- {
+ else {
// Found non pcd file
- PCL_DEBUG ("[PCDVideoPlayer::selectFolderButtonPressed] : found a different file\n");
+ PCL_DEBUG(
+ "[PCDVideoPlayer::selectFolderButtonPressed] : found a different file\n");
}
}
}
- else
- {
+ else {
PCL_ERROR("Path is not a directory\n");
exit(-1);
}
- nr_of_frames_ = pcd_files_.size ();
- PCL_DEBUG ("[PCDVideoPlayer::selectFolderButtonPressed] : found %d files\n", nr_of_frames_ );
+ nr_of_frames_ = pcd_files_.size();
+ PCL_DEBUG("[PCDVideoPlayer::selectFolderButtonPressed] : found %d files\n",
+ nr_of_frames_);
- if (nr_of_frames_ == 0)
- {
- PCL_ERROR ("Please select valid pcd folder\n");
+ if (nr_of_frames_ == 0) {
+ PCL_ERROR("Please select valid pcd folder\n");
cloud_present_ = false;
return;
}
// Reset the Slider
- ui_->indexSlider->setValue (0); // set cursor back in the beginning
- ui_->indexSlider->setRange (0, nr_of_frames_ - 1); // rescale the slider
+ ui_->indexSlider->setValue(0); // set cursor back in the beginning
+ ui_->indexSlider->setRange(0, nr_of_frames_ - 1); // rescale the slider
current_frame_ = 0;
cloud_modified_ = true;
}
-void
-PCDVideoPlayer::selectFilesButtonPressed ()
+void
+PCDVideoPlayer::selectFilesButtonPressed()
{
- pcd_files_.clear (); // Clear the std::vector
- pcd_paths_.clear (); // Clear the boost filesystem paths
+ pcd_files_.clear(); // Clear the std::vector
+ pcd_paths_.clear(); // Clear the boost filesystem paths
- QStringList qt_pcd_files = QFileDialog::getOpenFileNames (this,
- "Select one or more PCD files to open",
- "/home",
- "PointClouds (*.pcd)");
- nr_of_frames_ = qt_pcd_files.size ();
- PCL_INFO ("[PCDVideoPlayer::selectFilesButtonPressed] : selected %ld files\n", nr_of_frames_);
+ QStringList qt_pcd_files = QFileDialog::getOpenFileNames(
+ this, "Select one or more PCD files to open", "/home", "PointClouds (*.pcd)");
+ nr_of_frames_ = qt_pcd_files.size();
+ PCL_INFO("[PCDVideoPlayer::selectFilesButtonPressed] : selected %ld files\n",
+ nr_of_frames_);
- if (nr_of_frames_ == 0)
- {
- PCL_ERROR ("Please select valid pcd files\n");
+ if (nr_of_frames_ == 0) {
+ PCL_ERROR("Please select valid pcd files\n");
cloud_present_ = false;
return;
}
- for(int i = 0; i < qt_pcd_files.size (); i++)
- {
- pcd_files_.push_back (qt_pcd_files.at (i).toStdString ());
+ for (int i = 0; i < qt_pcd_files.size(); i++) {
+ pcd_files_.push_back(qt_pcd_files.at(i).toStdString());
}
current_frame_ = 0;
// Reset the Slider
- ui_->indexSlider->setValue (0); // set cursor back in the beginning
- ui_->indexSlider->setRange (0, nr_of_frames_ - 1); // rescale the slider
+ ui_->indexSlider->setValue(0); // set cursor back in the beginning
+ ui_->indexSlider->setRange(0, nr_of_frames_ - 1); // rescale the slider
cloud_present_ = true;
cloud_modified_ = true;
}
-void
-PCDVideoPlayer::timeoutSlot ()
+void
+PCDVideoPlayer::timeoutSlot()
{
- if (play_mode_)
- {
- if (speed_counter_ == speed_value_)
- {
- if (current_frame_ == (nr_of_frames_-1)) // Reached the end
+ if (play_mode_) {
+ if (speed_counter_ == speed_value_) {
+ if (current_frame_ == (nr_of_frames_ - 1)) // Reached the end
{
current_frame_ = 0; // reset to beginning
}
- else
- {
+ else {
current_frame_++;
cloud_modified_ = true;
- ui_->indexSlider->setSliderPosition (current_frame_); // Update the slider position
+ ui_->indexSlider->setSliderPosition(
+ current_frame_); // Update the slider position
}
}
- else
- {
+ else {
speed_counter_++;
}
}
- if (cloud_present_ && cloud_modified_)
- {
- if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (pcd_files_[current_frame_], *cloud_) == -1) //* load the file
+ if (cloud_present_ && cloud_modified_) {
+ if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(pcd_files_[current_frame_], *cloud_) ==
+ -1) //* load the file
{
- PCL_ERROR ("[PCDVideoPlayer::timeoutSlot] : Couldn't read file %s\n");
+ PCL_ERROR("[PCDVideoPlayer::timeoutSlot] : Couldn't read file %s\n");
}
- if (!vis_->updatePointCloud(cloud_, "cloud_"))
- {
- vis_->addPointCloud (cloud_, "cloud_");
+ if (!vis_->updatePointCloud(cloud_, "cloud_")) {
+ vis_->addPointCloud(cloud_, "cloud_");
vis_->resetCameraViewpoint("cloud_");
}
cloud_modified_ = false;
}
- ui_->qvtkWidget->update ();
+ ui_->qvtkWidget->update();
}
void
-PCDVideoPlayer::indexSliderValueChanged (int value)
+PCDVideoPlayer::indexSliderValueChanged(int value)
{
- PCL_DEBUG ("[PCDVideoPlayer::indexSliderValueChanged] : (I) : value %d\n", value);
+ PCL_DEBUG("[PCDVideoPlayer::indexSliderValueChanged] : (I) : value %d\n", value);
current_frame_ = value;
cloud_modified_ = true;
}
void
-print_usage ()
+print_usage()
{
+ // clang-format off
PCL_INFO ("PCDVideoPlayer V0.1\n");
PCL_INFO ("-------------------\n");
PCL_INFO ("\tThe slider accepts focus on Tab and provides both a mouse wheel and a keyboard interface. The keyboard interface is the following:\n");
PCL_INFO ("\t PageDown moves down one page.\n");
PCL_INFO ("\t Home moves to the start (minimum).\n");
PCL_INFO ("\t End moves to the end (maximum).\n");
+ // clang-format on
}
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
- QApplication app (argc, argv);
+ QApplication app(argc, argv);
PCDVideoPlayer VideoPlayer;
- VideoPlayer.show ();
+ VideoPlayer.show();
- return (QApplication::exec ());
+ return QApplication::exec();
}
-#include <thread>
-
+#include <pcl/features/normal_3d.h>
#include <pcl/features/ppf.h>
-#include <pcl/io/pcd_io.h>
+#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
-#include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/registration/ppf_registration.h>
-
-#include <pcl/visualization/pcl_visualizer.h>
-
-#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <thread>
using namespace pcl;
using namespace std;
using namespace std::chrono_literals;
-const Eigen::Vector4f subsampling_leaf_size (0.02f, 0.02f, 0.02f, 0.0f);
+const Eigen::Vector4f subsampling_leaf_size(0.02f, 0.02f, 0.02f, 0.0f);
const float normal_estimation_search_radius = 0.05f;
-
PointCloud<PointNormal>::Ptr
-subsampleAndCalculateNormals
(const PointCloud<PointXYZ>::Ptr& cloud)
+subsampleAndCalculateNormals(const PointCloud<PointXYZ>::Ptr& cloud)
{
- PointCloud<PointXYZ>::Ptr cloud_subsampled (new PointCloud<PointXYZ> ());
+ PointCloud<PointXYZ>::Ptr cloud_subsampled(new PointCloud<PointXYZ>());
VoxelGrid<PointXYZ> subsampling_filter;
- subsampling_filter.setInputCloud (cloud);
- subsampling_filter.setLeafSize (subsampling_leaf_size);
- subsampling_filter.filter (*cloud_subsampled);
+ subsampling_filter.setInputCloud(cloud);
+ subsampling_filter.setLeafSize(subsampling_leaf_size);
+ subsampling_filter.filter(*cloud_subsampled);
- PointCloud<Normal>::Ptr cloud_subsampled_normals (new PointCloud<Normal> ());
+ PointCloud<Normal>::Ptr cloud_subsampled_normals(new PointCloud<Normal>());
NormalEstimation<PointXYZ, Normal> normal_estimation_filter;
- normal_estimation_filter.setInputCloud (cloud_subsampled);
- search::KdTree<PointXYZ>::Ptr search_tree (new search::KdTree<PointXYZ>);
- normal_estimation_filter.setSearchMethod (search_tree);
- normal_estimation_filter.setRadiusSearch (normal_estimation_search_radius);
- normal_estimation_filter.compute (*cloud_subsampled_normals);
-
- PointCloud<PointNormal>::Ptr cloud_subsampled_with_normals (new PointCloud<PointNormal> ());
- concatenateFields (*cloud_subsampled, *cloud_subsampled_normals, *cloud_subsampled_with_normals);
-
- PCL_INFO ("Cloud dimensions before / after subsampling: %u / %u\n", cloud->points.size (), cloud_subsampled->points.size ());
+ normal_estimation_filter.setInputCloud(cloud_subsampled);
+ search::KdTree<PointXYZ>::Ptr search_tree(new search::KdTree<PointXYZ>);
+ normal_estimation_filter.setSearchMethod(search_tree);
+ normal_estimation_filter.setRadiusSearch(normal_estimation_search_radius);
+ normal_estimation_filter.compute(*cloud_subsampled_normals);
+
+ PointCloud<PointNormal>::Ptr cloud_subsampled_with_normals(
+ new PointCloud<PointNormal>());
+ concatenateFields(
+ *cloud_subsampled, *cloud_subsampled_normals, *cloud_subsampled_with_normals);
+
+ PCL_INFO("Cloud dimensions before / after subsampling: %u / %u\n",
+ cloud->points.size(),
+ cloud_subsampled->points.size());
return cloud_subsampled_with_normals;
}
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
- if (argc != 3)
- {
- PCL_ERROR ("Syntax: ./ppf_object_recognition pcd_model_list pcd_scene\n");
+ if (argc != 3) {
+ PCL_ERROR("Syntax: ./ppf_object_recognition pcd_model_list pcd_scene\n");
return -1;
}
-
/// read point clouds from HDD
- PCL_INFO ("Reading scene ...\n");
- PointCloud<PointXYZ>::Ptr cloud_scene (new PointCloud<PointXYZ> ());
+ PCL_INFO("Reading scene ...\n");
+ PointCloud<PointXYZ>::Ptr cloud_scene(new PointCloud<PointXYZ>());
PCDReader reader;
- reader.read (argv[2], *cloud_scene);
- PCL_INFO ("Scene read: %s\n", argv[2]);
-
- PCL_INFO ("Reading models ...\n");
- std::vector<PointCloud<PointXYZ>::Ptr > cloud_models;
- ifstream pcd_file_list (argv[1]);
- while (!pcd_file_list.eof())
- {
+ reader.read(argv[2], *cloud_scene);
+ PCL_INFO("Scene read: %s\n", argv[2]);
+
+ PCL_INFO("Reading models ...\n");
+ std::vector<PointCloud<PointXYZ>::Ptr> cloud_models;
+ ifstream pcd_file_list(argv[1]);
+ while (!pcd_file_list.eof()) {
char str[512];
- pcd_file_list.getline (str, 512);
- PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
- reader.read (str, *cloud);
- cloud_models.push_back (cloud);
- PCL_INFO ("Model read: %s\n", str);
+ pcd_file_list.getline(str, 512);
+ PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());
+ reader.read(str, *cloud);
+ cloud_models.push_back(cloud);
+ PCL_INFO("Model read: %s\n", str);
}
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::ExtractIndices<pcl::PointXYZ> extract;
- seg.setOptimizeCoefficients (true);
- seg.setModelType (pcl::SACMODEL_PLANE);
- seg.setMethodType (pcl::SAC_RANSAC);
- seg.setMaxIterations (1000);
- seg.setDistanceThreshold (0.05);
- extract.setNegative (true);
- pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
- pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
- unsigned nr_points = unsigned (cloud_scene->points.size ());
- while (cloud_scene->points.size () > 0.3 * nr_points)
- {
- seg.setInputCloud (cloud_scene);
- seg.segment (*inliers, *coefficients);
- PCL_INFO ("Plane inliers: %u\n", inliers->indices.size ());
- if (inliers->indices.size () < 50000) break;
-
- extract.setInputCloud (cloud_scene);
- extract.setIndices (inliers);
- extract.filter (*cloud_scene);
+ seg.setOptimizeCoefficients(true);
+ seg.setModelType(pcl::SACMODEL_PLANE);
+ seg.setMethodType(pcl::SAC_RANSAC);
+ seg.setMaxIterations(1000);
+ seg.setDistanceThreshold(0.05);
+ extract.setNegative(true);
+ pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
+ unsigned nr_points = unsigned(cloud_scene->points.size());
+ while (cloud_scene->points.size() > 0.3 * nr_points) {
+ seg.setInputCloud(cloud_scene);
+ seg.segment(*inliers, *coefficients);
+ PCL_INFO("Plane inliers: %u\n", inliers->indices.size());
+ if (inliers->indices.size() < 50000)
+ break;
+
+ extract.setInputCloud(cloud_scene);
+ extract.setIndices(inliers);
+ extract.filter(*cloud_scene);
}
- PointCloud<PointNormal>::Ptr cloud_scene_input = subsampleAndCalculateNormals (cloud_scene);
- std::vector<PointCloud<PointNormal>::Ptr > cloud_models_with_normals;
+ PointCloud<PointNormal>::Ptr cloud_scene_input =
+ subsampleAndCalculateNormals(cloud_scene);
+ std::vector<PointCloud<PointNormal>::Ptr> cloud_models_with_normals;
- PCL_INFO ("Training models ...\n");
+ PCL_INFO("Training models ...\n");
std::vector<PPFHashMapSearch::Ptr> hashmap_search_vector;
- for (const auto &cloud_model : cloud_models)
- {
- PointCloud<PointNormal>::Ptr cloud_model_input = subsampleAndCalculateNormals (cloud_model);
- cloud_models_with_normals.push_back (cloud_model_input);
+ for (const auto& cloud_model : cloud_models) {
+ PointCloud<PointNormal>::Ptr cloud_model_input =
+ subsampleAndCalculateNormals(cloud_model);
+ cloud_models_with_normals.push_back(cloud_model_input);
- PointCloud<PPFSignature>::Ptr cloud_model_ppf (new PointCloud<PPFSignature> ());
+ PointCloud<PPFSignature>::Ptr cloud_model_ppf(new PointCloud<PPFSignature>());
PPFEstimation<PointNormal, PointNormal, PPFSignature> ppf_estimator;
- ppf_estimator.setInputCloud (cloud_model_input);
- ppf_estimator.setInputNormals (cloud_model_input);
- ppf_estimator.compute (*cloud_model_ppf);
-
- PPFHashMapSearch::Ptr hashmap_search (new PPFHashMapSearch (12.0f / 180.0f * float (M_PI),
- 0.05f));
- hashmap_search->setInputFeatureCloud (cloud_model_ppf);
- hashmap_search_vector.push_back (hashmap_search);
+ ppf_estimator.setInputCloud(cloud_model_input);
+ ppf_estimator.setInputNormals(cloud_model_input);
+ ppf_estimator.compute(*cloud_model_ppf);
+
+ PPFHashMapSearch::Ptr hashmap_search(
+ new PPFHashMapSearch(12.0f / 180.0f * float(M_PI), 0.05f));
+ hashmap_search->setInputFeatureCloud(cloud_model_ppf);
+ hashmap_search_vector.push_back(hashmap_search);
}
-
- visualization::PCLVisualizer viewer ("PPF Object Recognition - Results");
- viewer.setBackgroundColor (0, 0, 0);
- viewer.addPointCloud (cloud_scene);
- viewer.spinOnce (10);
- PCL_INFO ("Registering models to scene ...\n");
- for (std::size_t model_i = 0; model_i < cloud_models.size (); ++model_i)
- {
+ visualization::PCLVisualizer viewer("PPF Object Recognition - Results");
+ viewer.setBackgroundColor(0, 0, 0);
+ viewer.addPointCloud(cloud_scene);
+ viewer.spinOnce(10);
+ PCL_INFO("Registering models to scene ...\n");
+ for (std::size_t model_i = 0; model_i < cloud_models.size(); ++model_i) {
PPFRegistration<PointNormal, PointNormal> ppf_registration;
// set parameters for the PPF registration procedure
- ppf_registration.setSceneReferencePointSamplingRate (10);
- ppf_registration.setPositionClusteringThreshold (0.2f);
- ppf_registration.setRotationClusteringThreshold (30.0f / 180.0f * float (M_PI));
- ppf_registration.setSearchMethod (hashmap_search_vector[model_i]);
- ppf_registration.setInputSource (cloud_models_with_normals[model_i]);
- ppf_registration.setInputTarget (cloud_scene_input);
+ ppf_registration.setSceneReferencePointSamplingRate(10);
+ ppf_registration.setPositionClusteringThreshold(0.2f);
+ ppf_registration.setRotationClusteringThreshold(30.0f / 180.0f * float(M_PI));
+ ppf_registration.setSearchMethod(hashmap_search_vector[model_i]);
+ ppf_registration.setInputSource(cloud_models_with_normals[model_i]);
+ ppf_registration.setInputTarget(cloud_scene_input);
PointCloud<PointNormal> cloud_output_subsampled;
- ppf_registration.align (cloud_output_subsampled);
+ ppf_registration.align(cloud_output_subsampled);
- PointCloud<PointXYZ>::Ptr cloud_output_subsampled_xyz (new PointCloud<PointXYZ> ());
- for (const auto &point : cloud_output_subsampled.points)
+ PointCloud<PointXYZ>::Ptr cloud_output_subsampled_xyz(new PointCloud<PointXYZ>());
+ for (const auto& point : cloud_output_subsampled.points)
cloud_output_subsampled_xyz->points.emplace_back(point.x, point.y, point.z);
+ Eigen::Matrix4f mat = ppf_registration.getFinalTransformation();
+ Eigen::Affine3f final_transformation(mat);
- Eigen::Matrix4f mat = ppf_registration.getFinalTransformation ();
- Eigen::Affine3f final_transformation (mat);
-
-
- // io::savePCDFileASCII ("output_subsampled_registered.pcd", cloud_output_subsampled);
+ PointCloud<PointXYZ>::Ptr cloud_output(new PointCloud<PointXYZ>());
+ pcl::transformPointCloud(
+ *cloud_models[model_i], *cloud_output, final_transformation);
- PointCloud<PointXYZ>::Ptr cloud_output (new PointCloud<PointXYZ> ());
- pcl::transformPointCloud (*cloud_models[model_i], *cloud_output, final_transformation);
-
-
- stringstream ss; ss << "model_" << model_i;
- visualization::PointCloudColorHandlerRandom<PointXYZ> random_color (cloud_output->makeShared ());
- viewer.addPointCloud (cloud_output, random_color, ss.str ());
-// io::savePCDFileASCII (ss.str ().c_str (), *cloud_output);
- PCL_INFO ("Showing model %s\n", ss.str ().c_str ());
+ stringstream ss;
+ ss << "model_" << model_i;
+ visualization::PointCloudColorHandlerRandom<PointXYZ> random_color(
+ cloud_output->makeShared());
+ viewer.addPointCloud(cloud_output, random_color, ss.str());
+ PCL_INFO("Showing model %s\n", ss.str().c_str());
}
- PCL_INFO ("All models have been registered!\n");
-
+ PCL_INFO("All models have been registered!\n");
- while (!viewer.wasStopped ())
- {
- viewer.spinOnce (100);
+ while (!viewer.wasStopped()) {
+ viewer.spinOnce(100);
std::this_thread::sleep_for(100ms);
}
-#include <pcl/io/pcd_io.h>
-#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/ppf.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/registration/pyramid_feature_matching.h>
using namespace pcl;
#include <iostream>
using namespace std;
-const Eigen::Vector4f subsampling_leaf_size (0.02f, 0.02f, 0.02f, 0.0f);
+const Eigen::Vector4f subsampling_leaf_size(0.02f, 0.02f, 0.02f, 0.0f);
const float normal_estimation_search_radius = 0.05f;
-//const Eigen::Vector4f subsampling_leaf_size (5, 5, 5, 0.0);
-//const float normal_estimation_search_radius = 11;
-
void
-subsampleAndCalculateNormals (PointCloud<PointXYZ>::Ptr &cloud,
- PointCloud<PointXYZ>::Ptr &cloud_subsampled,
- PointCloud<Normal>::Ptr &cloud_subsampled_normals)
+subsampleAndCalculateNormals(PointCloud<PointXYZ>::Ptr& cloud,
+ PointCloud<PointXYZ>::Ptr& cloud_subsampled,
+ PointCloud<Normal>::Ptr& cloud_subsampled_normals)
{
- cloud_subsampled = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ> ());
+ cloud_subsampled = PointCloud<PointXYZ>::Ptr(new PointCloud<PointXYZ>());
VoxelGrid<PointXYZ> subsampling_filter;
- subsampling_filter.setInputCloud (cloud);
- subsampling_filter.setLeafSize (subsampling_leaf_size);
- subsampling_filter.filter (*cloud_subsampled);
+ subsampling_filter.setInputCloud(cloud);
+ subsampling_filter.setLeafSize(subsampling_leaf_size);
+ subsampling_filter.filter(*cloud_subsampled);
- cloud_subsampled_normals = PointCloud<Normal>::Ptr (new PointCloud<Normal> ());
+ cloud_subsampled_normals = PointCloud<Normal>::Ptr(new PointCloud<Normal>());
NormalEstimation<PointXYZ, Normal> normal_estimation_filter;
- normal_estimation_filter.setInputCloud (cloud_subsampled);
- search::KdTree<PointXYZ>::Ptr search_tree (new search::KdTree<PointXYZ>);
- normal_estimation_filter.setSearchMethod (search_tree);
- normal_estimation_filter.setRadiusSearch (normal_estimation_search_radius);
- normal_estimation_filter.compute (*cloud_subsampled_normals);
-
- std::cerr << "Before -> After subsampling: " << cloud->points.size () << " -> " << cloud_subsampled->points.size () << std::endl;
+ normal_estimation_filter.setInputCloud(cloud_subsampled);
+ search::KdTree<PointXYZ>::Ptr search_tree(new search::KdTree<PointXYZ>);
+ normal_estimation_filter.setSearchMethod(search_tree);
+ normal_estimation_filter.setRadiusSearch(normal_estimation_search_radius);
+ normal_estimation_filter.compute(*cloud_subsampled_normals);
+
+ std::cerr << "Before -> After subsampling: " << cloud->points.size() << " -> "
+ << cloud_subsampled->points.size() << std::endl;
}
-
int
-main (int argc, char **argv)
+main(int argc, char** argv)
{
- if (argc != 3)
- {
- PCL_ERROR ("Syntax: ./pyramid_surface_matching model_1 model_2\n");
+ if (argc != 3) {
+ PCL_ERROR("Syntax: ./pyramid_surface_matching model_1 model_2\n");
return -1;
}
-
/// read point clouds from HDD
- PCL_INFO ("Reading scene ...\n");
- PointCloud<PointXYZ>::Ptr cloud_a (new PointCloud<PointXYZ> ()),
- cloud_b (new PointCloud<PointXYZ> ());
+ PCL_INFO("Reading scene ...\n");
+ PointCloud<PointXYZ>::Ptr cloud_a(new PointCloud<PointXYZ>()),
+ cloud_b(new PointCloud<PointXYZ>());
PCDReader reader;
- reader.read (argv[1], *cloud_a);
- reader.read (argv[2], *cloud_b);
+ reader.read(argv[1], *cloud_a);
+ reader.read(argv[2], *cloud_b);
PointCloud<PointXYZ>::Ptr cloud_a_subsampled, cloud_b_subsampled;
PointCloud<Normal>::Ptr cloud_a_subsampled_normals, cloud_b_subsampled_normals;
- subsampleAndCalculateNormals (cloud_a, cloud_a_subsampled, cloud_a_subsampled_normals);
- subsampleAndCalculateNormals (cloud_b, cloud_b_subsampled, cloud_b_subsampled_normals);
-
- PCL_INFO ("Finished subsampling the clouds ...\n");
-
-
- PointCloud<PPFSignature>::Ptr ppf_signature_a (new PointCloud<PPFSignature> ()),
- ppf_signature_b (new PointCloud<PPFSignature> ());
- PointCloud<PointNormal>::Ptr cloud_subsampled_with_normals_a (new PointCloud<PointNormal> ()),
- cloud_subsampled_with_normals_b (new PointCloud<PointNormal> ());
- concatenateFields (*cloud_a_subsampled, *cloud_a_subsampled_normals, *cloud_subsampled_with_normals_a);
- concatenateFields (*cloud_b_subsampled, *cloud_b_subsampled_normals, *cloud_subsampled_with_normals_b);
+ subsampleAndCalculateNormals(cloud_a, cloud_a_subsampled, cloud_a_subsampled_normals);
+ subsampleAndCalculateNormals(cloud_b, cloud_b_subsampled, cloud_b_subsampled_normals);
+
+ PCL_INFO("Finished subsampling the clouds ...\n");
+
+ PointCloud<PPFSignature>::Ptr ppf_signature_a(new PointCloud<PPFSignature>()),
+ ppf_signature_b(new PointCloud<PPFSignature>());
+ PointCloud<PointNormal>::Ptr cloud_subsampled_with_normals_a(
+ new PointCloud<PointNormal>()),
+ cloud_subsampled_with_normals_b(new PointCloud<PointNormal>());
+ concatenateFields(*cloud_a_subsampled,
+ *cloud_a_subsampled_normals,
+ *cloud_subsampled_with_normals_a);
+ concatenateFields(*cloud_b_subsampled,
+ *cloud_b_subsampled_normals,
+ *cloud_subsampled_with_normals_b);
PPFEstimation<PointNormal, PointNormal, PPFSignature> ppf_estimator;
- ppf_estimator.setInputCloud (cloud_subsampled_with_normals_a);
- ppf_estimator.setInputNormals (cloud_subsampled_with_normals_a);
- ppf_estimator.compute (*ppf_signature_a);
- ppf_estimator.setInputCloud (cloud_subsampled_with_normals_b);
- ppf_estimator.setInputNormals (cloud_subsampled_with_normals_b);
- ppf_estimator.compute (*ppf_signature_b);
-
- PCL_INFO ("Feature cloud sizes: %u , %u\n", ppf_signature_a->points.size (), ppf_signature_b->points.size ());
-
- PCL_INFO ("Finished calculating the features ...\n");
- std::vector<pair<float, float> > dim_range_input, dim_range_target;
- for (std::size_t i = 0; i < 3; ++i) dim_range_input.emplace_back(float (-M_PI), float (M_PI));
+ ppf_estimator.setInputCloud(cloud_subsampled_with_normals_a);
+ ppf_estimator.setInputNormals(cloud_subsampled_with_normals_a);
+ ppf_estimator.compute(*ppf_signature_a);
+ ppf_estimator.setInputCloud(cloud_subsampled_with_normals_b);
+ ppf_estimator.setInputNormals(cloud_subsampled_with_normals_b);
+ ppf_estimator.compute(*ppf_signature_b);
+
+ PCL_INFO("Feature cloud sizes: %u , %u\n",
+ ppf_signature_a->points.size(),
+ ppf_signature_b->points.size());
+
+ PCL_INFO("Finished calculating the features ...\n");
+ std::vector<pair<float, float>> dim_range_input, dim_range_target;
+ for (std::size_t i = 0; i < 3; ++i)
+ dim_range_input.emplace_back(float(-M_PI), float(M_PI));
dim_range_input.emplace_back(0.0f, 1.0f);
- for (std::size_t i = 0; i < 3; ++i) dim_range_target.emplace_back(float (-M_PI) * 10.0f, float (M_PI) * 10.0f);
+ for (std::size_t i = 0; i < 3; ++i)
+ dim_range_target.emplace_back(float(-M_PI) * 10.0f, float(M_PI) * 10.0f);
dim_range_target.emplace_back(0.0f, 50.0f);
-
- PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_a (new PyramidFeatureHistogram<PPFSignature> ());
- pyramid_a->setInputCloud (ppf_signature_a);
- pyramid_a->setInputDimensionRange (dim_range_input);
- pyramid_a->setTargetDimensionRange (dim_range_target);
- pyramid_a->compute ();
- PCL_INFO ("Done with the first pyramid\n");
-
- PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_b (new PyramidFeatureHistogram<PPFSignature> ());
- pyramid_b->setInputCloud (ppf_signature_b);
- pyramid_b->setInputDimensionRange (dim_range_input);
- pyramid_b->setTargetDimensionRange (dim_range_target);
- pyramid_b->compute ();
- PCL_INFO ("Done with the second pyramid\n");
-
- float value = PyramidFeatureHistogram<PPFSignature>::comparePyramidFeatureHistograms (pyramid_a, pyramid_b);
- PCL_INFO ("Surface comparison value between %s and %s is: %f\n", argv[1], argv[2], value);
-
+ PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_a(
+ new PyramidFeatureHistogram<PPFSignature>());
+ pyramid_a->setInputCloud(ppf_signature_a);
+ pyramid_a->setInputDimensionRange(dim_range_input);
+ pyramid_a->setTargetDimensionRange(dim_range_target);
+ pyramid_a->compute();
+ PCL_INFO("Done with the first pyramid\n");
+
+ PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_b(
+ new PyramidFeatureHistogram<PPFSignature>());
+ pyramid_b->setInputCloud(ppf_signature_b);
+ pyramid_b->setInputDimensionRange(dim_range_input);
+ pyramid_b->setTargetDimensionRange(dim_range_target);
+ pyramid_b->compute();
+ PCL_INFO("Done with the second pyramid\n");
+
+ float value = PyramidFeatureHistogram<PPFSignature>::comparePyramidFeatureHistograms(
+ pyramid_a, pyramid_b);
+ PCL_INFO(
+ "Surface comparison value between %s and %s is: %f\n", argv[1], argv[2], value);
return 0;
}
* Created on: Dec 23, 2011
* Author: aitor
*/
-#include <array>
-#include <pcl/point_types.h>
#include <pcl/apps/render_views_tesselated_sphere.h>
+#include <pcl/point_types.h>
+
+#include <vtkActor.h>
+#include <vtkCamera.h>
+#include <vtkCellArray.h>
#include <vtkCellData.h>
-#include <vtkWorldPointPicker.h>
-#include <vtkPropPicker.h>
-#include <vtkPlatonicSolidSource.h>
-#include <vtkLoopSubdivisionFilter.h>
-#include <vtkTriangle.h>
-#include <vtkTransform.h>
#include <vtkHardwareSelector.h>
-#include <vtkSelectionNode.h>
-#include <vtkSelection.h>
-#include <vtkCellArray.h>
-#include <vtkTransformFilter.h>
-#include <vtkCamera.h>
-#include <vtkActor.h>
+#include <vtkLoopSubdivisionFilter.h>
+#include <vtkPlatonicSolidSource.h>
+#include <vtkPointPicker.h>
+#include <vtkPolyDataMapper.h>
+#include <vtkPropPicker.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
-#include <vtkPolyDataMapper.h>
-#include <vtkPointPicker.h>
+#include <vtkSelection.h>
+#include <vtkSelectionNode.h>
+#include <vtkTransform.h>
+#include <vtkTransformFilter.h>
+#include <vtkTriangle.h>
+#include <vtkWorldPointPicker.h>
+
+#include <array>
void
-pcl::apps::RenderViewsTesselatedSphere::generateViews() {
- //center object
+pcl::apps::RenderViewsTesselatedSphere::generateViews()
+{
+ // center object
double CoM[3];
vtkIdType npts_com = 0, *ptIds_com = nullptr;
- vtkSmartPointer<vtkCellArray> cells_com = polydata_->GetPolys ();
+ vtkSmartPointer<vtkCellArray> cells_com = polydata_->GetPolys();
double center[3], p1_com[3], p2_com[3], p3_com[3], totalArea_com = 0;
double comx = 0, comy = 0, comz = 0;
- for (cells_com->InitTraversal (); cells_com->GetNextCell (npts_com, ptIds_com);)
- {
- polydata_->GetPoint (ptIds_com[0], p1_com);
- polydata_->GetPoint (ptIds_com[1], p2_com);
- polydata_->GetPoint (ptIds_com[2], p3_com);
- vtkTriangle::TriangleCenter (p1_com, p2_com, p3_com, center);
- double area_com = vtkTriangle::TriangleArea (p1_com, p2_com, p3_com);
+ for (cells_com->InitTraversal(); cells_com->GetNextCell(npts_com, ptIds_com);) {
+ polydata_->GetPoint(ptIds_com[0], p1_com);
+ polydata_->GetPoint(ptIds_com[1], p2_com);
+ polydata_->GetPoint(ptIds_com[2], p3_com);
+ vtkTriangle::TriangleCenter(p1_com, p2_com, p3_com, center);
+ double area_com = vtkTriangle::TriangleArea(p1_com, p2_com, p3_com);
comx += center[0] * area_com;
comy += center[1] * area_com;
comz += center[2] * area_com;
CoM[1] = comy / totalArea_com;
CoM[2] = comz / totalArea_com;
- vtkSmartPointer<vtkTransform> trans_center = vtkSmartPointer<vtkTransform>::New ();
- trans_center->Translate (-CoM[0], -CoM[1], -CoM[2]);
- vtkSmartPointer<vtkMatrix4x4> matrixCenter = trans_center->GetMatrix ();
+ vtkSmartPointer<vtkTransform> trans_center = vtkSmartPointer<vtkTransform>::New();
+ trans_center->Translate(-CoM[0], -CoM[1], -CoM[2]);
+ vtkSmartPointer<vtkMatrix4x4> matrixCenter = trans_center->GetMatrix();
- vtkSmartPointer<vtkTransformFilter> trans_filter_center = vtkSmartPointer<vtkTransformFilter>::New ();
- trans_filter_center->SetTransform (trans_center);
- trans_filter_center->SetInputData (polydata_);
- trans_filter_center->Update ();
+ vtkSmartPointer<vtkTransformFilter> trans_filter_center =
+ vtkSmartPointer<vtkTransformFilter>::New();
+ trans_filter_center->SetTransform(trans_center);
+ trans_filter_center->SetInputData(polydata_);
+ trans_filter_center->Update();
- vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
- mapper->SetInputConnection (trans_filter_center->GetOutputPort ());
- mapper->Update ();
+ vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+ mapper->SetInputConnection(trans_filter_center->GetOutputPort());
+ mapper->Update();
- //scale so it fits in the unit sphere!
+ // scale so it fits in the unit sphere!
double bb[6];
- mapper->GetBounds (bb);
- double ms = (std::max) ((std::fabs) (bb[0] - bb[1]),
- (std::max) ((std::fabs) (bb[2] - bb[3]), (std::fabs) (bb[4] - bb[5])));
+ mapper->GetBounds(bb);
+ double ms =
+ std::max((std::fabs)(bb[0] - bb[1]),
+ std::max((std::fabs)(bb[2] - bb[3]), (std::fabs)(bb[4] - bb[5])));
double max_side = radius_sphere_ / 2.0;
double scale_factor = max_side / ms;
- vtkSmartPointer<vtkTransform> trans_scale = vtkSmartPointer<vtkTransform>::New ();
- trans_scale->Scale (scale_factor, scale_factor, scale_factor);
- vtkSmartPointer<vtkMatrix4x4> matrixScale = trans_scale->GetMatrix ();
+ vtkSmartPointer<vtkTransform> trans_scale = vtkSmartPointer<vtkTransform>::New();
+ trans_scale->Scale(scale_factor, scale_factor, scale_factor);
+ vtkSmartPointer<vtkMatrix4x4> matrixScale = trans_scale->GetMatrix();
- vtkSmartPointer<vtkTransformFilter> trans_filter_scale = vtkSmartPointer<vtkTransformFilter>::New ();
- trans_filter_scale->SetTransform (trans_scale);
- trans_filter_scale->SetInputConnection (trans_filter_center->GetOutputPort ());
- trans_filter_scale->Update ();
+ vtkSmartPointer<vtkTransformFilter> trans_filter_scale =
+ vtkSmartPointer<vtkTransformFilter>::New();
+ trans_filter_scale->SetTransform(trans_scale);
+ trans_filter_scale->SetInputConnection(trans_filter_center->GetOutputPort());
+ trans_filter_scale->Update();
- mapper->SetInputConnection (trans_filter_scale->GetOutputPort ());
- mapper->Update ();
+ mapper->SetInputConnection(trans_filter_scale->GetOutputPort());
+ mapper->Update();
//////////////////////////////
// * Compute area of the mesh
//////////////////////////////
- vtkSmartPointer<vtkCellArray> cells = mapper->GetInput ()->GetPolys ();
+ vtkSmartPointer<vtkCellArray> cells = mapper->GetInput()->GetPolys();
vtkIdType npts = 0, *ptIds = nullptr;
double p1[3], p2[3], p3[3], totalArea = 0;
- for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds);)
- {
- polydata_->GetPoint (ptIds[0], p1);
- polydata_->GetPoint (ptIds[1], p2);
- polydata_->GetPoint (ptIds[2], p3);
- totalArea += vtkTriangle::TriangleArea (p1, p2, p3);
+ for (cells->InitTraversal(); cells->GetNextCell(npts, ptIds);) {
+ polydata_->GetPoint(ptIds[0], p1);
+ polydata_->GetPoint(ptIds[1], p2);
+ polydata_->GetPoint(ptIds[2], p3);
+ totalArea += vtkTriangle::TriangleArea(p1, p2, p3);
}
- //create icosahedron
- vtkSmartPointer<vtkPlatonicSolidSource> ico = vtkSmartPointer<vtkPlatonicSolidSource>::New ();
- ico->SetSolidTypeToIcosahedron ();
- ico->Update ();
-
- //tessellate cells from icosahedron
- vtkSmartPointer<vtkLoopSubdivisionFilter> subdivide = vtkSmartPointer<vtkLoopSubdivisionFilter>::New ();
- subdivide->SetNumberOfSubdivisions (tesselation_level_);
- subdivide->SetInputConnection (ico->GetOutputPort ());
+ // create icosahedron
+ vtkSmartPointer<vtkPlatonicSolidSource> ico =
+ vtkSmartPointer<vtkPlatonicSolidSource>::New();
+ ico->SetSolidTypeToIcosahedron();
+ ico->Update();
+
+ // tessellate cells from icosahedron
+ vtkSmartPointer<vtkLoopSubdivisionFilter> subdivide =
+ vtkSmartPointer<vtkLoopSubdivisionFilter>::New();
+ subdivide->SetNumberOfSubdivisions(tesselation_level_);
+ subdivide->SetInputConnection(ico->GetOutputPort());
subdivide->Update();
// Get camera positions
- vtkPolyData *sphere = subdivide->GetOutput ();
-
- std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > cam_positions;
- if (!use_vertices_)
- {
- vtkSmartPointer<vtkCellArray> cells_sphere = sphere->GetPolys ();
- cam_positions.resize (sphere->GetNumberOfPolys ());
-
- std::size_t i=0;
- for (cells_sphere->InitTraversal (); cells_sphere->GetNextCell (npts_com, ptIds_com);)
- {
- sphere->GetPoint (ptIds_com[0], p1_com);
- sphere->GetPoint (ptIds_com[1], p2_com);
- sphere->GetPoint (ptIds_com[2], p3_com);
- vtkTriangle::TriangleCenter (p1_com, p2_com, p3_com, center);
- cam_positions[i] = Eigen::Vector3f (float (center[0]), float (center[1]), float (center[2]));
+ vtkPolyData* sphere = subdivide->GetOutput();
+
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> cam_positions;
+ if (!use_vertices_) {
+ vtkSmartPointer<vtkCellArray> cells_sphere = sphere->GetPolys();
+ cam_positions.resize(sphere->GetNumberOfPolys());
+
+ std::size_t i = 0;
+ for (cells_sphere->InitTraversal();
+ cells_sphere->GetNextCell(npts_com, ptIds_com);) {
+ sphere->GetPoint(ptIds_com[0], p1_com);
+ sphere->GetPoint(ptIds_com[1], p2_com);
+ sphere->GetPoint(ptIds_com[2], p3_com);
+ vtkTriangle::TriangleCenter(p1_com, p2_com, p3_com, center);
+ cam_positions[i] =
+ Eigen::Vector3f(float(center[0]), float(center[1]), float(center[2]));
i++;
}
-
}
- else
- {
- cam_positions.resize (sphere->GetNumberOfPoints ());
- for (vtkIdType i = 0; i < sphere->GetNumberOfPoints (); i++)
- {
+ else {
+ cam_positions.resize(sphere->GetNumberOfPoints());
+ for (vtkIdType i = 0; i < sphere->GetNumberOfPoints(); i++) {
double cam_pos[3];
- sphere->GetPoint (i, cam_pos);
- cam_positions[i] = Eigen::Vector3f (float (cam_pos[0]), float (cam_pos[1]), float (cam_pos[2]));
+ sphere->GetPoint(i, cam_pos);
+ cam_positions[i] =
+ Eigen::Vector3f(float(cam_pos[0]), float(cam_pos[1]), float(cam_pos[2]));
}
}
first_cam_pos[1] = cam_positions[0][1] * radius_sphere_;
first_cam_pos[2] = cam_positions[0][2] * radius_sphere_;
- //create renderer and window
- vtkSmartPointer<vtkRenderWindow> render_win = vtkSmartPointer<vtkRenderWindow>::New ();
- vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New ();
- render_win->AddRenderer (renderer);
- render_win->SetSize (resolution_, resolution_);
- renderer->SetBackground (1.0, 0, 0);
+ // create renderer and window
+ vtkSmartPointer<vtkRenderWindow> render_win = vtkSmartPointer<vtkRenderWindow>::New();
+ vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
+ render_win->AddRenderer(renderer);
+ render_win->SetSize(resolution_, resolution_);
+ renderer->SetBackground(1.0, 0, 0);
- //create picker
- vtkSmartPointer<vtkWorldPointPicker> worldPicker = vtkSmartPointer<vtkWorldPointPicker>::New ();
+ // create picker
+ vtkSmartPointer<vtkWorldPointPicker> worldPicker =
+ vtkSmartPointer<vtkWorldPointPicker>::New();
- vtkSmartPointer<vtkCamera> cam = vtkSmartPointer<vtkCamera>::New ();
- cam->SetFocalPoint (0, 0, 0);
+ vtkSmartPointer<vtkCamera> cam = vtkSmartPointer<vtkCamera>::New();
+ cam->SetFocalPoint(0, 0, 0);
Eigen::Vector3f cam_pos_3f = cam_positions[0];
- Eigen::Vector3f perp = cam_pos_3f.cross (Eigen::Vector3f::UnitY ());
- cam->SetViewUp (perp[0], perp[1], perp[2]);
+ Eigen::Vector3f perp = cam_pos_3f.cross(Eigen::Vector3f::UnitY());
+ cam->SetViewUp(perp[0], perp[1], perp[2]);
- cam->SetPosition (first_cam_pos.data());
- cam->SetViewAngle (view_angle_);
- cam->Modified ();
+ cam->SetPosition(first_cam_pos.data());
+ cam->SetViewAngle(view_angle_);
+ cam->Modified();
- //For each camera position, traposesnsform the object and render view
- for (const auto &cam_position : cam_positions)
- {
+ // For each camera position, traposesnsform the object and render view
+ for (const auto& cam_position : cam_positions) {
cam_pos[0] = cam_position[0];
cam_pos[1] = cam_position[1];
cam_pos[2] = cam_position[2];
- //create temporal virtual camera
- vtkSmartPointer<vtkCamera> cam_tmp = vtkSmartPointer<vtkCamera>::New ();
- cam_tmp->SetViewAngle (view_angle_);
-
- Eigen::Vector3f cam_pos_3f (static_cast<float> (cam_pos[0]), static_cast<float> (cam_pos[1]), static_cast<float> (cam_pos[2]));
- cam_pos_3f = cam_pos_3f.normalized ();
- Eigen::Vector3f test = Eigen::Vector3f::UnitY ();
-
- //If the view up is parallel to ray cam_pos - focalPoint then the transformation
- //is singular and no points are rendered...
- //make sure it is perpendicular
- if (std::abs (cam_pos_3f.dot (test)) == 1)
- {
- //parallel, create
- test = cam_pos_3f.cross (Eigen::Vector3f::UnitX ());
+ // create temporal virtual camera
+ vtkSmartPointer<vtkCamera> cam_tmp = vtkSmartPointer<vtkCamera>::New();
+ cam_tmp->SetViewAngle(view_angle_);
+
+ Eigen::Vector3f cam_pos_3f(static_cast<float>(cam_pos[0]),
+ static_cast<float>(cam_pos[1]),
+ static_cast<float>(cam_pos[2]));
+ cam_pos_3f = cam_pos_3f.normalized();
+ Eigen::Vector3f test = Eigen::Vector3f::UnitY();
+
+ // If the view up is parallel to ray cam_pos - focalPoint then the transformation
+ // is singular and no points are rendered...
+ // make sure it is perpendicular
+ if (std::abs(cam_pos_3f.dot(test)) == 1) {
+ // parallel, create
+ test = cam_pos_3f.cross(Eigen::Vector3f::UnitX());
}
- cam_tmp->SetViewUp (test[0], test[1], test[2]);
+ cam_tmp->SetViewUp(test[0], test[1], test[2]);
- for (double &cam_po : cam_pos)
- {
+ for (double& cam_po : cam_pos) {
cam_po *= camera_radius;
}
- cam_tmp->SetPosition (cam_pos.data());
- cam_tmp->SetFocalPoint (0, 0, 0);
- cam_tmp->Modified ();
-
- //rotate model so it looks the same as if we would look from the new position
- vtkSmartPointer<vtkMatrix4x4> view_trans_inverted = vtkSmartPointer<vtkMatrix4x4>::New ();
- vtkMatrix4x4::Invert (cam->GetViewTransformMatrix (), view_trans_inverted);
- vtkSmartPointer<vtkTransform> trans_rot_pose = vtkSmartPointer<vtkTransform>::New ();
- trans_rot_pose->Identity ();
- trans_rot_pose->Concatenate (view_trans_inverted);
- trans_rot_pose->Concatenate (cam_tmp->GetViewTransformMatrix ());
- vtkSmartPointer<vtkTransformFilter> trans_rot_pose_filter = vtkSmartPointer<vtkTransformFilter>::New ();
- trans_rot_pose_filter->SetTransform (trans_rot_pose);
- trans_rot_pose_filter->SetInputConnection (trans_filter_scale->GetOutputPort ());
-
- //translate model so we can place camera at (0,0,0)
- vtkSmartPointer<vtkTransform> translation = vtkSmartPointer<vtkTransform>::New ();
- translation->Translate (first_cam_pos[0] * -1, first_cam_pos[1] * -1, first_cam_pos[2] * -1);
- vtkSmartPointer<vtkTransformFilter> translation_filter = vtkSmartPointer<vtkTransformFilter>::New ();
- translation_filter->SetTransform (translation);
- translation_filter->SetInputConnection (trans_rot_pose_filter->GetOutputPort ());
-
- //modify camera
- cam_tmp->SetPosition (0, 0, 0);
- cam_tmp->SetFocalPoint (first_cam_pos[0] * -1, first_cam_pos[1] * -1, first_cam_pos[2] * -1);
- cam_tmp->Modified ();
-
- //notice transformations for final pose
- vtkSmartPointer<vtkMatrix4x4> matrixRotModel = trans_rot_pose->GetMatrix ();
- vtkSmartPointer<vtkMatrix4x4> matrixTranslation = translation->GetMatrix ();
-
- mapper->SetInputConnection (translation_filter->GetOutputPort ());
- mapper->Update ();
-
- //render view
- vtkSmartPointer<vtkActor> actor_view = vtkSmartPointer<vtkActor>::New ();
- actor_view->SetMapper (mapper);
- renderer->SetActiveCamera (cam_tmp);
- renderer->AddActor (actor_view);
- renderer->Modified ();
- //renderer->ResetCameraClippingRange ();
- render_win->Render ();
-
- //back to real scale transform
- vtkSmartPointer<vtkTransform> backToRealScale = vtkSmartPointer<vtkTransform>::New ();
- backToRealScale->PostMultiply ();
- backToRealScale->Identity ();
- backToRealScale->Concatenate (matrixScale);
- backToRealScale->Concatenate (matrixTranslation);
- backToRealScale->Inverse ();
- backToRealScale->Modified ();
- backToRealScale->Concatenate (matrixTranslation);
- backToRealScale->Modified ();
+ cam_tmp->SetPosition(cam_pos.data());
+ cam_tmp->SetFocalPoint(0, 0, 0);
+ cam_tmp->Modified();
+
+ // rotate model so it looks the same as if we would look from the new position
+ vtkSmartPointer<vtkMatrix4x4> view_trans_inverted =
+ vtkSmartPointer<vtkMatrix4x4>::New();
+ vtkMatrix4x4::Invert(cam->GetViewTransformMatrix(), view_trans_inverted);
+ vtkSmartPointer<vtkTransform> trans_rot_pose = vtkSmartPointer<vtkTransform>::New();
+ trans_rot_pose->Identity();
+ trans_rot_pose->Concatenate(view_trans_inverted);
+ trans_rot_pose->Concatenate(cam_tmp->GetViewTransformMatrix());
+ vtkSmartPointer<vtkTransformFilter> trans_rot_pose_filter =
+ vtkSmartPointer<vtkTransformFilter>::New();
+ trans_rot_pose_filter->SetTransform(trans_rot_pose);
+ trans_rot_pose_filter->SetInputConnection(trans_filter_scale->GetOutputPort());
+
+ // translate model so we can place camera at (0,0,0)
+ vtkSmartPointer<vtkTransform> translation = vtkSmartPointer<vtkTransform>::New();
+ translation->Translate(
+ first_cam_pos[0] * -1, first_cam_pos[1] * -1, first_cam_pos[2] * -1);
+ vtkSmartPointer<vtkTransformFilter> translation_filter =
+ vtkSmartPointer<vtkTransformFilter>::New();
+ translation_filter->SetTransform(translation);
+ translation_filter->SetInputConnection(trans_rot_pose_filter->GetOutputPort());
+
+ // modify camera
+ cam_tmp->SetPosition(0, 0, 0);
+ cam_tmp->SetFocalPoint(
+ first_cam_pos[0] * -1, first_cam_pos[1] * -1, first_cam_pos[2] * -1);
+ cam_tmp->Modified();
+
+ // notice transformations for final pose
+ vtkSmartPointer<vtkMatrix4x4> matrixRotModel = trans_rot_pose->GetMatrix();
+ vtkSmartPointer<vtkMatrix4x4> matrixTranslation = translation->GetMatrix();
+
+ mapper->SetInputConnection(translation_filter->GetOutputPort());
+ mapper->Update();
+
+ // render view
+ vtkSmartPointer<vtkActor> actor_view = vtkSmartPointer<vtkActor>::New();
+ actor_view->SetMapper(mapper);
+ renderer->SetActiveCamera(cam_tmp);
+ renderer->AddActor(actor_view);
+ renderer->Modified();
+ // renderer->ResetCameraClippingRange ();
+ render_win->Render();
+
+ // back to real scale transform
+ vtkSmartPointer<vtkTransform> backToRealScale =
+ vtkSmartPointer<vtkTransform>::New();
+ backToRealScale->PostMultiply();
+ backToRealScale->Identity();
+ backToRealScale->Concatenate(matrixScale);
+ backToRealScale->Concatenate(matrixTranslation);
+ backToRealScale->Inverse();
+ backToRealScale->Modified();
+ backToRealScale->Concatenate(matrixTranslation);
+ backToRealScale->Modified();
Eigen::Matrix4f backToRealScale_eigen;
- backToRealScale_eigen.setIdentity ();
+ backToRealScale_eigen.setIdentity();
for (int x = 0; x < 4; x++)
for (int y = 0; y < 4; y++)
- backToRealScale_eigen (x, y) = float (backToRealScale->GetMatrix ()->GetElement (x, y));
+ backToRealScale_eigen(x, y) =
+ float(backToRealScale->GetMatrix()->GetElement(x, y));
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
(new pcl::PointCloud<pcl::PointXYZ>);
- cloud->points.resize (resolution_ * resolution_);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ cloud->points.resize(resolution_ * resolution_);
- if (gen_organized_)
- {
+ if (gen_organized_) {
cloud->width = resolution_;
cloud->height = resolution_;
cloud->is_dense = false;
double coords[3];
- float * depth = new float[resolution_ * resolution_];
- render_win->GetZbufferData (0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));
+ float* depth = new float[resolution_ * resolution_];
+ render_win->GetZbufferData(0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));
- for (int x = 0; x < resolution_; x++)
- {
- for (int y = 0; y < resolution_; y++)
- {
+ for (int x = 0; x < resolution_; x++) {
+ for (int y = 0; y < resolution_; y++) {
float value = depth[y * resolution_ + x];
- if (value == 1.0)
- {
- cloud->at (y, x).x = cloud->at (y, x).y = cloud->at (y, x).z = std::numeric_limits<float>::quiet_NaN ();
+ if (value == 1.0) {
+ cloud->at(y, x).x = cloud->at(y, x).y = cloud->at(y, x).z =
+ std::numeric_limits<float>::quiet_NaN();
}
- else
- {
- worldPicker->Pick (x, y, value, renderer);
- worldPicker->GetPickPosition (coords);
- cloud->at (y, x).x = static_cast<float> (coords[0]);
- cloud->at (y, x).y = static_cast<float> (coords[1]);
- cloud->at (y, x).z = static_cast<float> (coords[2]);
- cloud->at (y, x).getVector4fMap () = backToRealScale_eigen
- * cloud->at (y, x).getVector4fMap ();
+ else {
+ worldPicker->Pick(x, y, value, renderer);
+ worldPicker->GetPickPosition(coords);
+ cloud->at(y, x).x = static_cast<float>(coords[0]);
+ cloud->at(y, x).y = static_cast<float>(coords[1]);
+ cloud->at(y, x).z = static_cast<float>(coords[2]);
+ cloud->at(y, x).getVector4fMap() =
+ backToRealScale_eigen * cloud->at(y, x).getVector4fMap();
}
}
}
delete[] depth;
-
}
- else
- {
+ else {
cloud->width = resolution_ * resolution_;
cloud->height = 1;
double coords[3];
- float * depth = new float[resolution_ * resolution_];
- render_win->GetZbufferData (0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));
+ float* depth = new float[resolution_ * resolution_];
+ render_win->GetZbufferData(0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));
int count_valid_depth_pixels = 0;
- for (int x = 0; x < resolution_; x++)
- {
- for (int y = 0; y < resolution_; y++)
- {
+ for (int x = 0; x < resolution_; x++) {
+ for (int y = 0; y < resolution_; y++) {
float value = depth[y * resolution_ + x];
if (value == 1.0)
continue;
- worldPicker->Pick (x, y, value, renderer);
- worldPicker->GetPickPosition (coords);
- cloud->points[count_valid_depth_pixels].x = static_cast<float> (coords[0]);
- cloud->points[count_valid_depth_pixels].y = static_cast<float> (coords[1]);
- cloud->points[count_valid_depth_pixels].z = static_cast<float> (coords[2]);
- cloud->points[count_valid_depth_pixels].getVector4fMap () = backToRealScale_eigen
- * cloud->points[count_valid_depth_pixels].getVector4fMap ();
+ worldPicker->Pick(x, y, value, renderer);
+ worldPicker->GetPickPosition(coords);
+ cloud->points[count_valid_depth_pixels].x = static_cast<float>(coords[0]);
+ cloud->points[count_valid_depth_pixels].y = static_cast<float>(coords[1]);
+ cloud->points[count_valid_depth_pixels].z = static_cast<float>(coords[2]);
+ cloud->points[count_valid_depth_pixels].getVector4fMap() =
+ backToRealScale_eigen *
+ cloud->points[count_valid_depth_pixels].getVector4fMap();
count_valid_depth_pixels++;
}
}
delete[] depth;
- cloud->points.resize (count_valid_depth_pixels);
+ cloud->points.resize(count_valid_depth_pixels);
cloud->width = count_valid_depth_pixels;
}
- if(compute_entropy_) {
+ if (compute_entropy_) {
//////////////////////////////
// * Compute area of the mesh
//////////////////////////////
- vtkSmartPointer<vtkPolyData> polydata = mapper->GetInput ();
- polydata->BuildCells ();
+ vtkSmartPointer<vtkPolyData> polydata = mapper->GetInput();
+ polydata->BuildCells();
- vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
+ vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys();
vtkIdType npts = 0, *ptIds = nullptr;
double p1[3], p2[3], p3[3], area, totalArea = 0;
- for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds);)
- {
- polydata->GetPoint (ptIds[0], p1);
- polydata->GetPoint (ptIds[1], p2);
- polydata->GetPoint (ptIds[2], p3);
- area = vtkTriangle::TriangleArea (p1, p2, p3);
+ for (cells->InitTraversal(); cells->GetNextCell(npts, ptIds);) {
+ polydata->GetPoint(ptIds[0], p1);
+ polydata->GetPoint(ptIds[1], p2);
+ polydata->GetPoint(ptIds[2], p3);
+ area = vtkTriangle::TriangleArea(p1, p2, p3);
totalArea += area;
}
/////////////////////////////////////
// * Select visible cells (triangles)
/////////////////////////////////////
- vtkSmartPointer<vtkHardwareSelector> hardware_selector = vtkSmartPointer<vtkHardwareSelector>::New ();
+ vtkSmartPointer<vtkHardwareSelector> hardware_selector =
+ vtkSmartPointer<vtkHardwareSelector>::New();
hardware_selector->ClearBuffers();
- vtkSmartPointer<vtkSelection> hdw_selection = vtkSmartPointer<vtkSelection>::New ();
- hardware_selector->SetRenderer (renderer);
- hardware_selector->SetArea (0, 0, resolution_ - 1, resolution_ - 1);
+ vtkSmartPointer<vtkSelection> hdw_selection =
+ vtkSmartPointer<vtkSelection>::New();
+ hardware_selector->SetRenderer(renderer);
+ hardware_selector->SetArea(0, 0, resolution_ - 1, resolution_ - 1);
hardware_selector->SetFieldAssociation(vtkDataObject::FIELD_ASSOCIATION_CELLS);
- hdw_selection = hardware_selector->Select ();
- vtkSmartPointer<vtkIdTypeArray> ids = vtkSmartPointer<vtkIdTypeArray>::New ();
+ hdw_selection = hardware_selector->Select();
+ vtkSmartPointer<vtkIdTypeArray> ids = vtkSmartPointer<vtkIdTypeArray>::New();
ids = vtkIdTypeArray::SafeDownCast(hdw_selection->GetNode(0)->GetSelectionList());
double visible_area = 0;
- for (vtkIdType sel_id = 0; sel_id < (ids->GetNumberOfTuples ()); sel_id++)
- {
- int id_mesh = int (ids->GetValue (sel_id));
- if(id_mesh >= polydata->GetNumberOfPolys())
+ for (vtkIdType sel_id = 0; sel_id < (ids->GetNumberOfTuples()); sel_id++) {
+ int id_mesh = int(ids->GetValue(sel_id));
+ if (id_mesh >= polydata->GetNumberOfPolys())
continue;
- vtkCell * cell = polydata->GetCell (id_mesh);
- vtkTriangle* triangle = dynamic_cast<vtkTriangle*> (cell);
+ vtkCell* cell = polydata->GetCell(id_mesh);
+ vtkTriangle* triangle = dynamic_cast<vtkTriangle*>(cell);
double p0[3];
double p1[3];
double p2[3];
- triangle->GetPoints ()->GetPoint (0, p0);
- triangle->GetPoints ()->GetPoint (1, p1);
- triangle->GetPoints ()->GetPoint (2, p2);
- area = vtkTriangle::TriangleArea (p0, p1, p2);
+ triangle->GetPoints()->GetPoint(0, p0);
+ triangle->GetPoints()->GetPoint(1, p1);
+ triangle->GetPoints()->GetPoint(2, p2);
+ area = vtkTriangle::TriangleArea(p0, p1, p2);
visible_area += area;
}
- entropies_.push_back (float (visible_area / totalArea));
+ entropies_.push_back(float(visible_area / totalArea));
}
- //transform cloud to give camera coordinates instead of world coordinates!
- vtkSmartPointer<vtkMatrix4x4> view_transform = cam_tmp->GetViewTransformMatrix ();
+ // transform cloud to give camera coordinates instead of world coordinates!
+ vtkSmartPointer<vtkMatrix4x4> view_transform = cam_tmp->GetViewTransformMatrix();
Eigen::Matrix4f trans_view;
- trans_view.setIdentity ();
+ trans_view.setIdentity();
for (int x = 0; x < 4; x++)
for (int y = 0; y < 4; y++)
- trans_view (x, y) = float (view_transform->GetElement (x, y));
+ trans_view(x, y) = float(view_transform->GetElement(x, y));
- //NOTE: vtk view coordinate system is different than the standard camera coordinates (z forward, y down, x right)
- //thus, the fliping in y and z
- for (auto &point : cloud->points)
- {
- point.getVector4fMap () = trans_view * point.getVector4fMap ();
+ // NOTE: vtk view coordinate system is different than the standard camera
+ // coordinates (z forward, y down, x right) thus, the fliping in y and z
+ for (auto& point : cloud->points) {
+ point.getVector4fMap() = trans_view * point.getVector4fMap();
point.y *= -1.0f;
point.z *= -1.0f;
}
- renderer->RemoveActor (actor_view);
+ renderer->RemoveActor(actor_view);
- generated_views_.push_back (cloud);
+ generated_views_.push_back(cloud);
- //create pose, from OBJECT coordinates to CAMERA coordinates!
- vtkSmartPointer<vtkTransform> transOCtoCC = vtkSmartPointer<vtkTransform>::New ();
- transOCtoCC->PostMultiply ();
- transOCtoCC->Identity ();
- transOCtoCC->Concatenate (matrixCenter);
- transOCtoCC->Concatenate (matrixRotModel);
- transOCtoCC->Concatenate (matrixTranslation);
- transOCtoCC->Concatenate (cam_tmp->GetViewTransformMatrix ());
+ // create pose, from OBJECT coordinates to CAMERA coordinates!
+ vtkSmartPointer<vtkTransform> transOCtoCC = vtkSmartPointer<vtkTransform>::New();
+ transOCtoCC->PostMultiply();
+ transOCtoCC->Identity();
+ transOCtoCC->Concatenate(matrixCenter);
+ transOCtoCC->Concatenate(matrixRotModel);
+ transOCtoCC->Concatenate(matrixTranslation);
+ transOCtoCC->Concatenate(cam_tmp->GetViewTransformMatrix());
- //NOTE: vtk view coordinate system is different than the standard camera coordinates (z forward, y down, x right)
- //thus, the fliping in y and z
- vtkSmartPointer<vtkMatrix4x4> cameraSTD = vtkSmartPointer<vtkMatrix4x4>::New ();
- cameraSTD->Identity ();
- cameraSTD->SetElement (0, 0, 1);
- cameraSTD->SetElement (1, 1, -1);
- cameraSTD->SetElement (2, 2, -1);
+ // NOTE: vtk view coordinate system is different than the standard camera
+ // coordinates (z forward, y down, x right) thus, the fliping in y and z
+ vtkSmartPointer<vtkMatrix4x4> cameraSTD = vtkSmartPointer<vtkMatrix4x4>::New();
+ cameraSTD->Identity();
+ cameraSTD->SetElement(0, 0, 1);
+ cameraSTD->SetElement(1, 1, -1);
+ cameraSTD->SetElement(2, 2, -1);
- transOCtoCC->Concatenate (cameraSTD);
- transOCtoCC->Modified ();
+ transOCtoCC->Concatenate(cameraSTD);
+ transOCtoCC->Modified();
Eigen::Matrix4f pose_view;
- pose_view.setIdentity ();
+ pose_view.setIdentity();
for (int x = 0; x < 4; x++)
for (int y = 0; y < 4; y++)
- pose_view (x, y) = float (transOCtoCC->GetMatrix ()->GetElement (x, y));
-
- poses_.push_back (pose_view);
+ pose_view(x, y) = float(transOCtoCC->GetMatrix()->GetElement(x, y));
+ poses_.push_back(pose_view);
}
}
*/
#include <pcl/features/statistical_multiscale_interest_region_extraction.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
-
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
using namespace pcl;
using namespace std;
const float subsampling_leaf_size = 0.003f;
const float base_scale = 0.005f;
-
int
-main (int, char **argv)
+main(int, char** argv)
{
- PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
+ PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());
PCDReader reader;
- reader.read (argv[1], *cloud);
- PCL_INFO ("Cloud read: %s\n", argv[1]);
- std::cerr << "cloud has #points: " << cloud->points.size () << std::endl;
+ reader.read(argv[1], *cloud);
+ PCL_INFO("Cloud read: %s\n", argv[1]);
+ std::cerr << "cloud has #points: " << cloud->points.size() << std::endl;
- PointCloud<PointXYZ>::Ptr cloud_subsampled (new PointCloud<PointXYZ> ());
+ PointCloud<PointXYZ>::Ptr cloud_subsampled(new PointCloud<PointXYZ>());
VoxelGrid<PointXYZ> subsampling_filter;
- subsampling_filter.setInputCloud (cloud);
- subsampling_filter.setLeafSize (subsampling_leaf_size, subsampling_leaf_size, subsampling_leaf_size);
- subsampling_filter.filter (*cloud_subsampled);
- std::cerr << "subsampled cloud has #points: " << cloud_subsampled->points.size () << std::endl;
+ subsampling_filter.setInputCloud(cloud);
+ subsampling_filter.setLeafSize(
+ subsampling_leaf_size, subsampling_leaf_size, subsampling_leaf_size);
+ subsampling_filter.filter(*cloud_subsampled);
+ std::cerr << "subsampled cloud has #points: " << cloud_subsampled->points.size()
+ << std::endl;
StatisticalMultiscaleInterestRegionExtraction<PointXYZ> region_extraction;
std::vector<float> scale_vector;
- PCL_INFO ("Scale values that will be used: ");
+ PCL_INFO("Scale values that will be used: ");
float base_scale_aux = base_scale;
- for (std::size_t scales = 0; scales < 7; ++scales)
- {
- PCL_INFO ("%f ", base_scale_aux);
- scale_vector.push_back (base_scale_aux);
+ for (std::size_t scales = 0; scales < 7; ++scales) {
+ PCL_INFO("%f ", base_scale_aux);
+ scale_vector.push_back(base_scale_aux);
base_scale_aux *= 1.6f;
}
- PCL_INFO ("\n");
- region_extraction.setInputCloud (cloud_subsampled);
- region_extraction.setScalesVector (scale_vector);
+ PCL_INFO("\n");
+ region_extraction.setInputCloud(cloud_subsampled);
+ region_extraction.setScalesVector(scale_vector);
std::list<IndicesPtr> rois;
- region_extraction.computeRegionsOfInterest (rois);
+ region_extraction.computeRegionsOfInterest(rois);
- PCL_INFO ("Regions of interest found: %d\n", rois.size ());
+ PCL_INFO("Regions of interest found: %d\n", rois.size());
pcl::ExtractIndices<PointXYZ> extract_indices_filter;
unsigned int roi_count = 0;
- for (const auto &roi : rois)
- {
+ for (const auto& roi : rois) {
PointCloud<PointXYZ> roi_points;
- extract_indices_filter.setInputCloud (cloud_subsampled);
- extract_indices_filter.setIndices (roi);
- extract_indices_filter.filter (roi_points);
+ extract_indices_filter.setInputCloud(cloud_subsampled);
+ extract_indices_filter.setIndices(roi);
+ extract_indices_filter.filter(roi_points);
char filename[512];
- sprintf (filename, "roi_%03d.pcd", ++roi_count);
- io::savePCDFileASCII (filename, roi_points);
+ sprintf(filename, "roi_%03d.pcd", ++roi_count);
+ io::savePCDFileASCII(filename, roi_points);
}
- io::savePCDFileASCII ("subsampled_input.pcd", *cloud_subsampled);
+ io::savePCDFileASCII("subsampled_input.pcd", *cloud_subsampled);
return 0;
}
*
*/
-#include <pcl/io/openni_grabber.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/image_viewer.h>
-#include <pcl/io/io.h>
-#include <pcl/io/ply_io.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/io/png_io.h>
-#include <pcl/io/pcd_grabber.h>
-#include <pcl/common/time.h>
#include <pcl/common/distances.h>
#include <pcl/common/intersections.h>
+#include <pcl/common/time.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
-#include <pcl/ModelCoefficients.h>
-#include <pcl/segmentation/planar_region.h>
-#include <pcl/segmentation/organized_multi_plane_segmentation.h>
-#include <pcl/segmentation/organized_connected_component_segmentation.h>
#include <pcl/filters/extract_indices.h>
+#include <pcl/io/io.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/png_io.h>
#include <pcl/sample_consensus/sac_model_plane.h>
-#include <pcl/stereo/stereo_matching.h>
-#include <pcl/segmentation/ground_plane_comparator.h>
#include <pcl/segmentation/euclidean_cluster_comparator.h>
+#include <pcl/segmentation/ground_plane_comparator.h>
+#include <pcl/segmentation/organized_connected_component_segmentation.h>
+#include <pcl/segmentation/organized_multi_plane_segmentation.h>
+#include <pcl/segmentation/planar_region.h>
+#include <pcl/stereo/stereo_matching.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/ModelCoefficients.h>
#include <mutex>
using CloudPtr = Cloud::Ptr;
using CloudConstPtr = Cloud::ConstPtr;
- /** \brief StereoGroundSegmentation is a demonstration application for using PCL's stereo tools and segmentation tools to detect smooth surfaces suitable for driving.
- *
- * \author Alex Trevor
- */
-class HRCSSegmentation
-{
- private:
- pcl::visualization::PCLVisualizer::Ptr viewer;
- pcl::visualization::ImageViewer::Ptr image_viewer;
- pcl::PointCloud<PointT>::ConstPtr prev_cloud;
- pcl::PointCloud<pcl::Normal>::ConstPtr prev_normal_cloud;
- pcl::PointCloud<pcl::PointXYZ>::ConstPtr prev_ground_cloud;
- pcl::PointCloud<PointT>::ConstPtr prev_ground_image;
- pcl::PointCloud<PointT>::ConstPtr prev_label_image;
- Eigen::Vector4f prev_ground_normal;
- Eigen::Vector4f prev_ground_centroid;
- std::mutex cloud_mutex;
-
- pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
- pcl::GroundPlaneComparator<PointT, pcl::Normal>::Ptr road_comparator;
- pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label> road_segmentation;
- std::vector<std::string> left_images;
- std::vector<std::string> right_images;
- int files_idx;
- int images_idx;
-
- pcl::AdaptiveCostSOStereoMatching stereo;
- bool trigger;
- bool continuous;
- bool display_normals;
- bool detect_obstacles;
- int smooth_weak;
- int smooth_strong;
-
- public:
- HRCSSegmentation (const std::vector<std::string> &left_images, const std::vector<std::string> &right_images) :
- viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")),
- image_viewer (new pcl::visualization::ImageViewer ("Image Viewer")),
- prev_cloud (new pcl::PointCloud<PointT>),
- prev_normal_cloud (new pcl::PointCloud<pcl::Normal>),
- prev_ground_cloud (new pcl::PointCloud<pcl::PointXYZ>),
- prev_ground_image (new pcl::PointCloud<PointT>),
- prev_label_image (new pcl::PointCloud<PointT>),
- road_comparator (new pcl::GroundPlaneComparator<PointT, pcl::Normal>),
- road_segmentation (road_comparator)
- {
- trigger = true;
- continuous = false;
- display_normals = false;
- detect_obstacles = false;
-
- this->left_images = left_images;
- this->right_images = right_images;
- files_idx = 0;
- images_idx = 0;
-
- // Set up a 3D viewer
- viewer->setBackgroundColor (0, 0, 0);
- viewer->addCoordinateSystem (1.0, "global");
- viewer->initCameraParameters ();
- viewer->registerKeyboardCallback (&HRCSSegmentation::keyboardCallback, *this, nullptr);
-
- // Set up the stereo matching
- stereo.setMaxDisparity(60);
- stereo.setXOffset(0);
- stereo.setRadius(5);
-
- smooth_weak = 20;
- smooth_strong = 100;
- stereo.setSmoothWeak(smooth_weak);
- stereo.setSmoothStrong(smooth_strong);
- stereo.setGammaC(25);
- stereo.setGammaS(10);
-
- stereo.setRatioFilter(20);
- stereo.setPeakFilter(0);
-
- stereo.setLeftRightCheck(true);
- stereo.setLeftRightCheckThreshold(1);
-
- stereo.setPreProcessing(true);
-
- // Set up the normal estimation
- ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
- ne.setMaxDepthChangeFactor (0.03f);
- ne.setNormalSmoothingSize (40.0f);//20.0f
-
- // Set up the groundplane comparator
- // If the camera was pointing straight out, the normal would be:
- Eigen::Vector3f nominal_road_normal (0.0, -1.0, 0.0);
- // Adjust for camera tilt:
- Eigen::Vector3f tilt_road_normal = Eigen::AngleAxisf (pcl::deg2rad (5.0f), Eigen::Vector3f::UnitX ()) * nominal_road_normal;
- road_comparator->setExpectedGroundNormal (tilt_road_normal);
- road_comparator->setGroundAngularThreshold (pcl::deg2rad (10.0f));
- road_comparator->setAngularThreshold (pcl::deg2rad (3.0f));
- }
-
- ~HRCSSegmentation ()
- {
+/** \brief StereoGroundSegmentation is a demonstration application for using PCL's
+ * stereo tools and segmentation tools to detect smooth surfaces suitable for driving.
+ *
+ * \author Alex Trevor
+ */
+class HRCSSegmentation {
+private:
+ pcl::visualization::PCLVisualizer::Ptr viewer;
+ pcl::visualization::ImageViewer::Ptr image_viewer;
+ pcl::PointCloud<PointT>::ConstPtr prev_cloud;
+ pcl::PointCloud<pcl::Normal>::ConstPtr prev_normal_cloud;
+ pcl::PointCloud<pcl::PointXYZ>::ConstPtr prev_ground_cloud;
+ pcl::PointCloud<PointT>::ConstPtr prev_ground_image;
+ pcl::PointCloud<PointT>::ConstPtr prev_label_image;
+ Eigen::Vector4f prev_ground_normal;
+ Eigen::Vector4f prev_ground_centroid;
+ std::mutex cloud_mutex;
+
+ pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+ pcl::GroundPlaneComparator<PointT, pcl::Normal>::Ptr road_comparator;
+ pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label> road_segmentation;
+ std::vector<std::string> left_images;
+ std::vector<std::string> right_images;
+ int files_idx;
+ int images_idx;
+
+ pcl::AdaptiveCostSOStereoMatching stereo;
+ bool trigger;
+ bool continuous;
+ bool display_normals;
+ bool detect_obstacles;
+ int smooth_weak;
+ int smooth_strong;
+
+public:
+ HRCSSegmentation(const std::vector<std::string>& left_images,
+ const std::vector<std::string>& right_images)
+ : viewer(new pcl::visualization::PCLVisualizer("3D Viewer"))
+ , image_viewer(new pcl::visualization::ImageViewer("Image Viewer"))
+ , prev_cloud(new pcl::PointCloud<PointT>)
+ , prev_normal_cloud(new pcl::PointCloud<pcl::Normal>)
+ , prev_ground_cloud(new pcl::PointCloud<pcl::PointXYZ>)
+ , prev_ground_image(new pcl::PointCloud<PointT>)
+ , prev_label_image(new pcl::PointCloud<PointT>)
+ , road_comparator(new pcl::GroundPlaneComparator<PointT, pcl::Normal>)
+ , road_segmentation(road_comparator)
+ {
+ trigger = true;
+ continuous = false;
+ display_normals = false;
+ detect_obstacles = false;
+
+ this->left_images = left_images;
+ this->right_images = right_images;
+ files_idx = 0;
+ images_idx = 0;
+
+ // Set up a 3D viewer
+ viewer->setBackgroundColor(0, 0, 0);
+ viewer->addCoordinateSystem(1.0, "global");
+ viewer->initCameraParameters();
+ viewer->registerKeyboardCallback(
+ &HRCSSegmentation::keyboardCallback, *this, nullptr);
+
+ // Set up the stereo matching
+ stereo.setMaxDisparity(60);
+ stereo.setXOffset(0);
+ stereo.setRadius(5);
+
+ smooth_weak = 20;
+ smooth_strong = 100;
+ stereo.setSmoothWeak(smooth_weak);
+ stereo.setSmoothStrong(smooth_strong);
+ stereo.setGammaC(25);
+ stereo.setGammaS(10);
+
+ stereo.setRatioFilter(20);
+ stereo.setPeakFilter(0);
+
+ stereo.setLeftRightCheck(true);
+ stereo.setLeftRightCheckThreshold(1);
+
+ stereo.setPreProcessing(true);
+
+ // Set up the normal estimation
+ ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
+ ne.setMaxDepthChangeFactor(0.03f);
+ ne.setNormalSmoothingSize(40.0f); // 20.0f
+
+ // Set up the groundplane comparator
+ // If the camera was pointing straight out, the normal would be:
+ Eigen::Vector3f nominal_road_normal(0.0, -1.0, 0.0);
+ // Adjust for camera tilt:
+ Eigen::Vector3f tilt_road_normal =
+ Eigen::AngleAxisf(pcl::deg2rad(5.0f), Eigen::Vector3f::UnitX()) *
+ nominal_road_normal;
+ road_comparator->setExpectedGroundNormal(tilt_road_normal);
+ road_comparator->setGroundAngularThreshold(pcl::deg2rad(10.0f));
+ road_comparator->setAngularThreshold(pcl::deg2rad(3.0f));
+ }
+
+ ~HRCSSegmentation() {}
+
+ void
+ cloud_cb_(const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ if (!viewer->wasStopped()) {
+ cloud_mutex.lock();
+ prev_cloud = cloud;
+ cloud_mutex.unlock();
}
-
- void
- cloud_cb_ (const pcl::PointCloud<PointT>::ConstPtr& cloud)
- {
- if (!viewer->wasStopped ())
- {
- cloud_mutex.lock ();
- prev_cloud = cloud;
- cloud_mutex.unlock ();
+ }
+
+ void
+ keyboardCallback(const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ if (event.keyUp()) {
+ switch (event.getKeyCode()) {
+ case ' ':
+ trigger = true;
+ break;
+ case '1':
+ smooth_strong -= 10;
+ PCL_INFO("smooth_strong: %d\n", smooth_strong);
+ stereo.setSmoothStrong(smooth_strong);
+ break;
+ case '2':
+ smooth_strong += 10;
+ PCL_INFO("smooth_strong: %d\n", smooth_strong);
+ stereo.setSmoothStrong(smooth_strong);
+ break;
+ case '3':
+ smooth_weak -= 10;
+ PCL_INFO("smooth_weak: %d\n", smooth_weak);
+ stereo.setSmoothWeak(smooth_weak);
+ break;
+ case '4':
+ smooth_weak += 10;
+ PCL_INFO("smooth_weak: %d\n", smooth_weak);
+ stereo.setSmoothWeak(smooth_weak);
+ break;
+ case 'c':
+ continuous = !continuous;
+ break;
+ case 'n':
+ display_normals = !display_normals;
+ break;
+ case 'o':
+ detect_obstacles = !detect_obstacles;
+ break;
}
}
+ }
- void
- keyboardCallback (const pcl::visualization::KeyboardEvent& event, void*)
- {
- if (event.keyUp ())
- {
- switch (event.getKeyCode ())
- {
- case ' ':
- trigger = true;
- break;
- case '1':
- smooth_strong -= 10;
- PCL_INFO ("smooth_strong: %d\n", smooth_strong);
- stereo.setSmoothStrong (smooth_strong);
- break;
- case '2':
- smooth_strong += 10;
- PCL_INFO ("smooth_strong: %d\n", smooth_strong);
- stereo.setSmoothStrong (smooth_strong);
- break;
- case '3':
- smooth_weak -= 10;
- PCL_INFO ("smooth_weak: %d\n", smooth_weak);
- stereo.setSmoothWeak (smooth_weak);
- break;
- case '4':
- smooth_weak += 10;
- PCL_INFO ("smooth_weak: %d\n", smooth_weak);
- stereo.setSmoothWeak (smooth_weak);
- break;
- case 'c':
- continuous = !continuous;
- break;
- case 'n':
- display_normals = !display_normals;
- break;
- case 'o':
- detect_obstacles = !detect_obstacles;
- break;
+ void
+ processStereoPair(const pcl::PointCloud<pcl::RGB>::Ptr& left_image,
+ const pcl::PointCloud<pcl::RGB>::Ptr& right_image,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr& out_cloud)
+ {
+ stereo.compute(*left_image, *right_image);
+ stereo.medianFilter(4);
+ stereo.getPointCloud(
+ 318.112200f, 224.334900f, 368.534700f, 0.8387445f, out_cloud, left_image);
+ }
+
+ void
+ processCloud(const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ // Compute the normals
+ pcl::PointCloud<pcl::Normal>::Ptr normal_cloud(new pcl::PointCloud<pcl::Normal>);
+ ne.setInputCloud(cloud);
+ ne.compute(*normal_cloud);
+
+ // Set up the groundplane comparator
+ road_comparator->setInputCloud(cloud);
+ road_comparator->setInputNormals(normal_cloud);
+
+ // Run segmentation
+ pcl::PointCloud<pcl::Label> labels;
+ std::vector<pcl::PointIndices> region_indices;
+ road_segmentation.setInputCloud(cloud);
+ road_segmentation.segment(labels, region_indices);
+
+ // Draw the segmentation result
+ pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud(
+ new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr ground_image(
+ new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr label_image(
+ new pcl::PointCloud<pcl::PointXYZRGB>);
+ *ground_image = *cloud;
+ *label_image = *cloud;
+
+ Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero();
+ Eigen::Vector4f vp = Eigen::Vector4f::Zero();
+ Eigen::Matrix3f clust_cov;
+ pcl::ModelCoefficients model;
+ model.values.resize(4);
+
+ std::vector<pcl::ModelCoefficients> model_coefficients;
+ std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> centroids;
+ std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>> covariances;
+ std::vector<pcl::PointIndices> inlier_indices;
+
+ for (const auto& region_index : region_indices) {
+ if (region_index.indices.size() > 1000) {
+
+ for (std::size_t j = 0; j < region_index.indices.size(); j++) {
+ pcl::PointXYZ ground_pt(cloud->points[region_index.indices[j]].x,
+ cloud->points[region_index.indices[j]].y,
+ cloud->points[region_index.indices[j]].z);
+ ground_cloud->points.push_back(ground_pt);
+ ground_image->points[region_index.indices[j]].g = static_cast<std::uint8_t>(
+ (cloud->points[region_index.indices[j]].g + 255) / 2);
+ label_image->points[region_index.indices[j]].r = 0;
+ label_image->points[region_index.indices[j]].g = 255;
+ label_image->points[region_index.indices[j]].b = 0;
+ }
+
+ // Compute plane info
+ pcl::computeMeanAndCovarianceMatrix(
+ *cloud, region_index.indices, clust_cov, clust_centroid);
+ Eigen::Vector4f plane_params;
+
+ EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
+ EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
+ pcl::eigen33(clust_cov, eigen_value, eigen_vector);
+ plane_params[0] = eigen_vector[0];
+ plane_params[1] = eigen_vector[1];
+ plane_params[2] = eigen_vector[2];
+ plane_params[3] = 0;
+ plane_params[3] = -1 * plane_params.dot(clust_centroid);
+
+ vp -= clust_centroid;
+ float cos_theta = vp.dot(plane_params);
+ if (cos_theta < 0) {
+ plane_params *= -1;
+ plane_params[3] = 0;
+ plane_params[3] = -1 * plane_params.dot(clust_centroid);
}
+
+ model.values[0] = plane_params[0];
+ model.values[1] = plane_params[1];
+ model.values[2] = plane_params[2];
+ model.values[3] = plane_params[3];
+ model_coefficients.push_back(model);
+ inlier_indices.push_back(region_index);
+ centroids.push_back(clust_centroid);
+ covariances.push_back(clust_cov);
}
}
-
- void
- processStereoPair (const pcl::PointCloud<pcl::RGB>::Ptr& left_image, const pcl::PointCloud<pcl::RGB>::Ptr& right_image, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& out_cloud)
- {
- stereo.compute (*left_image, *right_image);
- stereo.medianFilter (4);
- stereo.getPointCloud(318.112200f, 224.334900f, 368.534700f, 0.8387445f, out_cloud, left_image);
+ // Refinement
+ std::vector<bool> grow_labels;
+ std::vector<int> label_to_model;
+ grow_labels.resize(region_indices.size(), false);
+ label_to_model.resize(region_indices.size(), 0);
+
+ for (std::size_t i = 0; i < model_coefficients.size(); i++) {
+ int model_label = (labels)[inlier_indices[i].indices[0]].label;
+ label_to_model[model_label] = static_cast<int>(i);
+ grow_labels[model_label] = true;
}
-
- void
- processCloud (const pcl::PointCloud<PointT>::ConstPtr& cloud)
- {
- // Compute the normals
- pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
- ne.setInputCloud (cloud);
- ne.compute (*normal_cloud);
-
- // Set up the groundplane comparator
- road_comparator->setInputCloud (cloud);
- road_comparator->setInputNormals (normal_cloud);
-
- // Run segmentation
- pcl::PointCloud<pcl::Label> labels;
- std::vector<pcl::PointIndices> region_indices;
- road_segmentation.setInputCloud (cloud);
- road_segmentation.segment (labels, region_indices);
-
- // Draw the segmentation result
- pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud (new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr ground_image (new pcl::PointCloud<pcl::PointXYZRGB>);
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr label_image (new pcl::PointCloud<pcl::PointXYZRGB>);
- *ground_image = *cloud;
- *label_image = *cloud;
-
- Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
- Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
- Eigen::Matrix3f clust_cov;
- pcl::ModelCoefficients model;
- model.values.resize (4);
-
- std::vector<pcl::ModelCoefficients> model_coefficients;
- std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
- std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
- std::vector<pcl::PointIndices> inlier_indices;
-
- for (const auto ®ion_index : region_indices)
- {
- if (region_index.indices.size () > 1000)
- {
-
- for (std::size_t j = 0; j < region_index.indices.size (); j++)
- {
- pcl::PointXYZ ground_pt (cloud->points[region_index.indices[j]].x,
- cloud->points[region_index.indices[j]].y,
- cloud->points[region_index.indices[j]].z);
- ground_cloud->points.push_back (ground_pt);
- ground_image->points[region_index.indices[j]].g = static_cast<std::uint8_t> ((cloud->points[region_index.indices[j]].g + 255) / 2);
- label_image->points[region_index.indices[j]].r = 0;
- label_image->points[region_index.indices[j]].g = 255;
+ pcl::PointCloud<pcl::Label>::Ptr labels_ptr(new pcl::PointCloud<pcl::Label>);
+ *labels_ptr = labels;
+ pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
+ pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr
+ refinement_compare(
+ new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>());
+ refinement_compare->setInputCloud(cloud);
+ refinement_compare->setDistanceThreshold(0.15f);
+ refinement_compare->setLabels(labels_ptr);
+ refinement_compare->setModelCoefficients(model_coefficients);
+ refinement_compare->setRefineLabels(grow_labels);
+ refinement_compare->setLabelToModel(label_to_model);
+ mps.setRefinementComparator(refinement_compare);
+ mps.setMinInliers(500);
+ mps.setAngularThreshold(pcl::deg2rad(3.0));
+ mps.setDistanceThreshold(0.02);
+ mps.setInputCloud(cloud);
+ mps.setInputNormals(normal_cloud);
+ mps.refine(model_coefficients, inlier_indices, labels_ptr, region_indices);
+
+ // Note the regions that have been extended
+ pcl::PointCloud<PointT> extended_ground_cloud;
+ for (const auto& region_index : region_indices) {
+ if (region_index.indices.size() > 1000) {
+ for (std::size_t j = 0; j < region_index.indices.size(); j++) {
+ // Check to see if it has already been labeled
+ if (ground_image->points[region_index.indices[j]].g ==
+ ground_image->points[region_index.indices[j]].b) {
+ pcl::PointXYZ ground_pt(cloud->points[region_index.indices[j]].x,
+ cloud->points[region_index.indices[j]].y,
+ cloud->points[region_index.indices[j]].z);
+ ground_cloud->points.push_back(ground_pt);
+ ground_image->points[region_index.indices[j]].r = static_cast<std::uint8_t>(
+ (cloud->points[region_index.indices[j]].r + 255) / 2);
+ ground_image->points[region_index.indices[j]].g = static_cast<std::uint8_t>(
+ (cloud->points[region_index.indices[j]].g + 255) / 2);
+ label_image->points[region_index.indices[j]].r = 128;
+ label_image->points[region_index.indices[j]].g = 128;
label_image->points[region_index.indices[j]].b = 0;
- }
-
- // Compute plane info
- pcl::computeMeanAndCovarianceMatrix (*cloud, region_index.indices, clust_cov, clust_centroid);
- Eigen::Vector4f plane_params;
-
- EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
- EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
- pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
- plane_params[0] = eigen_vector[0];
- plane_params[1] = eigen_vector[1];
- plane_params[2] = eigen_vector[2];
- plane_params[3] = 0;
- plane_params[3] = -1 * plane_params.dot (clust_centroid);
-
- vp -= clust_centroid;
- float cos_theta = vp.dot (plane_params);
- if (cos_theta < 0)
- {
- plane_params *= -1;
- plane_params[3] = 0;
- plane_params[3] = -1 * plane_params.dot (clust_centroid);
- }
-
- model.values[0] = plane_params[0];
- model.values[1] = plane_params[1];
- model.values[2] = plane_params[2];
- model.values[3] = plane_params[3];
- model_coefficients.push_back (model);
- inlier_indices.push_back (region_index);
- centroids.push_back (clust_centroid);
- covariances.push_back (clust_cov);
- }
- }
-
- //Refinement
- std::vector<bool> grow_labels;
- std::vector<int> label_to_model;
- grow_labels.resize (region_indices.size (), false);
- label_to_model.resize (region_indices.size (), 0);
-
- for (std::size_t i = 0; i < model_coefficients.size (); i++)
- {
- int model_label = (labels)[inlier_indices[i].indices[0]].label;
- label_to_model[model_label] = static_cast<int> (i);
- grow_labels[model_label] = true;
- }
-
- pcl::PointCloud<pcl::Label>::Ptr labels_ptr (new pcl::PointCloud<pcl::Label>);
- *labels_ptr = labels;
- pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
- pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>());
- refinement_compare->setInputCloud (cloud);
- refinement_compare->setDistanceThreshold (0.15f);
- refinement_compare->setLabels (labels_ptr);
- refinement_compare->setModelCoefficients (model_coefficients);
- refinement_compare->setRefineLabels (grow_labels);
- refinement_compare->setLabelToModel (label_to_model);
- mps.setRefinementComparator (refinement_compare);
- mps.setMinInliers (500);
- mps.setAngularThreshold (pcl::deg2rad (3.0));
- mps.setDistanceThreshold (0.02);
- mps.setInputCloud (cloud);
- mps.setInputNormals (normal_cloud);
- mps.refine (model_coefficients,
- inlier_indices,
- labels_ptr,
- region_indices);
-
- //Note the regions that have been extended
- pcl::PointCloud<PointT> extended_ground_cloud;
- for (const auto ®ion_index : region_indices)
- {
- if (region_index.indices.size () > 1000)
- {
- for (std::size_t j = 0; j < region_index.indices.size (); j++)
- {
- // Check to see if it has already been labeled
- if (ground_image->points[region_index.indices[j]].g == ground_image->points[region_index.indices[j]].b)
- {
- pcl::PointXYZ ground_pt (cloud->points[region_index.indices[j]].x,
- cloud->points[region_index.indices[j]].y,
- cloud->points[region_index.indices[j]].z);
- ground_cloud->points.push_back (ground_pt);
- ground_image->points[region_index.indices[j]].r = static_cast<std::uint8_t> ((cloud->points[region_index.indices[j]].r + 255) / 2);
- ground_image->points[region_index.indices[j]].g = static_cast<std::uint8_t> ((cloud->points[region_index.indices[j]].g + 255) / 2);
- label_image->points[region_index.indices[j]].r = 128;
- label_image->points[region_index.indices[j]].g = 128;
- label_image->points[region_index.indices[j]].b = 0;
- }
-
}
}
}
+ }
- // Segment Obstacles (Disabled by default)
- Eigen::Vector4f ground_plane_params (1.0, 0.0, 0.0, 1.0);
- Eigen::Vector4f ground_centroid (0.0, 0.0, 0.0, 0.0);
-
- if (!ground_cloud->points.empty ())
- {
- ground_centroid = centroids[0];
- ground_plane_params = Eigen::Vector4f (model_coefficients[0].values[0], model_coefficients[0].values[1], model_coefficients[0].values[2], model_coefficients[0].values[3]);
- }
+ // Segment Obstacles (Disabled by default)
+ Eigen::Vector4f ground_plane_params(1.0, 0.0, 0.0, 1.0);
+ Eigen::Vector4f ground_centroid(0.0, 0.0, 0.0, 0.0);
- if (detect_obstacles)
- {
- pcl::PointCloud<PointT>::CloudVectorType clusters;
- if (!ground_cloud->points.empty ())
- {
- pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSetPtr plane_labels (new pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSet);
- for (std::size_t i = 0; i < region_indices.size (); ++i)
- if ((region_indices[i].indices.size () > mps.getMinInliers ()))
- plane_labels->insert (i);
-
- pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator<PointT, pcl::Label> ());
- euclidean_cluster_comparator_->setInputCloud (cloud);
- euclidean_cluster_comparator_->setLabels (labels_ptr);
- euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
- euclidean_cluster_comparator_->setDistanceThreshold (0.05f, false);
-
- pcl::PointCloud<pcl::Label> euclidean_labels;
- std::vector<pcl::PointIndices> euclidean_label_indices;
- pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
- euclidean_segmentation.setInputCloud (cloud);
- euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
-
- for (const auto &euclidean_label_index : euclidean_label_indices)
- {
- if ((euclidean_label_index.indices.size () > 200))
- {
- pcl::PointCloud<PointT> cluster;
- pcl::copyPointCloud (*cloud, euclidean_label_index.indices, cluster);
- clusters.push_back (cluster);
-
- Eigen::Vector4f cluster_centroid;
- Eigen::Matrix3f cluster_cov;
- pcl::computeMeanAndCovarianceMatrix (*cloud, euclidean_label_index.indices, cluster_cov, cluster_centroid);
-
- pcl::PointXYZ centroid_pt (cluster_centroid[0], cluster_centroid[1], cluster_centroid[2]);
- double ptp_dist = pcl::pointToPlaneDistanceSigned (centroid_pt, ground_plane_params[0], ground_plane_params[1], ground_plane_params[2], ground_plane_params[3]);
-
- if ((ptp_dist > 0.5) && (ptp_dist < 3.0))
- {
-
- for (std::size_t j = 0; j < euclidean_label_index.indices.size (); j++)
- {
- ground_image->points[euclidean_label_index.indices[j]].r = 255;
- label_image->points[euclidean_label_index.indices[j]].r = 255;
- label_image->points[euclidean_label_index.indices[j]].g = 0;
- label_image->points[euclidean_label_index.indices[j]].b = 0;
- }
+ if (!ground_cloud->points.empty()) {
+ ground_centroid = centroids[0];
+ ground_plane_params = Eigen::Vector4f(model_coefficients[0].values[0],
+ model_coefficients[0].values[1],
+ model_coefficients[0].values[2],
+ model_coefficients[0].values[3]);
+ }
+ if (detect_obstacles) {
+ pcl::PointCloud<PointT>::CloudVectorType clusters;
+ if (!ground_cloud->points.empty()) {
+ pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSetPtr
+ plane_labels(
+ new pcl::EuclideanClusterComparator<PointT,
+ pcl::Label>::ExcludeLabelSet);
+ for (std::size_t i = 0; i < region_indices.size(); ++i)
+ if ((region_indices[i].indices.size() > mps.getMinInliers()))
+ plane_labels->insert(i);
+
+ pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr
+ euclidean_cluster_comparator_(
+ new pcl::EuclideanClusterComparator<PointT, pcl::Label>());
+ euclidean_cluster_comparator_->setInputCloud(cloud);
+ euclidean_cluster_comparator_->setLabels(labels_ptr);
+ euclidean_cluster_comparator_->setExcludeLabels(plane_labels);
+ euclidean_cluster_comparator_->setDistanceThreshold(0.05f, false);
+
+ pcl::PointCloud<pcl::Label> euclidean_labels;
+ std::vector<pcl::PointIndices> euclidean_label_indices;
+ pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label>
+ euclidean_segmentation(euclidean_cluster_comparator_);
+ euclidean_segmentation.setInputCloud(cloud);
+ euclidean_segmentation.segment(euclidean_labels, euclidean_label_indices);
+
+ for (const auto& euclidean_label_index : euclidean_label_indices) {
+ if ((euclidean_label_index.indices.size() > 200)) {
+ pcl::PointCloud<PointT> cluster;
+ pcl::copyPointCloud(*cloud, euclidean_label_index.indices, cluster);
+ clusters.push_back(cluster);
+
+ Eigen::Vector4f cluster_centroid;
+ Eigen::Matrix3f cluster_cov;
+ pcl::computeMeanAndCovarianceMatrix(
+ *cloud, euclidean_label_index.indices, cluster_cov, cluster_centroid);
+
+ pcl::PointXYZ centroid_pt(
+ cluster_centroid[0], cluster_centroid[1], cluster_centroid[2]);
+ double ptp_dist = pcl::pointToPlaneDistanceSigned(centroid_pt,
+ ground_plane_params[0],
+ ground_plane_params[1],
+ ground_plane_params[2],
+ ground_plane_params[3]);
+
+ if ((ptp_dist > 0.5) && (ptp_dist < 3.0)) {
+
+ for (std::size_t j = 0; j < euclidean_label_index.indices.size(); j++) {
+ ground_image->points[euclidean_label_index.indices[j]].r = 255;
+ label_image->points[euclidean_label_index.indices[j]].r = 255;
+ label_image->points[euclidean_label_index.indices[j]].g = 0;
+ label_image->points[euclidean_label_index.indices[j]].b = 0;
}
-
- }
+ }
}
-
- }
- }
-
- // note the NAN points in the image as well
- for (std::size_t i = 0; i < cloud->points.size (); i++)
- {
- if (!pcl::isFinite (cloud->points[i]))
- {
- ground_image->points[i].b = static_cast<std::uint8_t>((cloud->points[i].b + 255) / 2);
- label_image->points[i].r = 0;
- label_image->points[i].g = 0;
- label_image->points[i].b = 255;
}
}
+ }
- // Update info for the visualization thread
- {
- cloud_mutex.lock ();
- prev_cloud = cloud;
- prev_normal_cloud = normal_cloud;
- prev_ground_cloud = ground_cloud;
- prev_ground_image = ground_image;
- prev_label_image = label_image;
- prev_ground_normal = ground_plane_params;
- prev_ground_centroid = ground_centroid;
- cloud_mutex.unlock ();
+ // note the NAN points in the image as well
+ for (std::size_t i = 0; i < cloud->points.size(); i++) {
+ if (!pcl::isFinite(cloud->points[i])) {
+ ground_image->points[i].b =
+ static_cast<std::uint8_t>((cloud->points[i].b + 255) / 2);
+ label_image->points[i].r = 0;
+ label_image->points[i].g = 0;
+ label_image->points[i].b = 255;
}
}
- void
- run ()
+ // Update info for the visualization thread
{
- while (!viewer->wasStopped ())
- {
- // Process a new image
- if (trigger || continuous)
- {
- pcl::PointCloud<pcl::RGB>::Ptr left_cloud (new pcl::PointCloud<pcl::RGB>);
- pcl::PointCloud<pcl::RGB>::Ptr right_cloud (new pcl::PointCloud<pcl::RGB>);
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
-
- pcl::PCDReader pcd;
- pcd.read (left_images[images_idx], *left_cloud);
- pcd.read (right_images[images_idx], *right_cloud);
- processStereoPair (left_cloud, right_cloud, out_cloud);
- processCloud (out_cloud);
- images_idx++;
-
- trigger = false;
+ cloud_mutex.lock();
+ prev_cloud = cloud;
+ prev_normal_cloud = normal_cloud;
+ prev_ground_cloud = ground_cloud;
+ prev_ground_image = ground_image;
+ prev_label_image = label_image;
+ prev_ground_normal = ground_plane_params;
+ prev_ground_centroid = ground_centroid;
+ cloud_mutex.unlock();
+ }
+ }
+
+ void
+ run()
+ {
+ while (!viewer->wasStopped()) {
+ // Process a new image
+ if (trigger || continuous) {
+ pcl::PointCloud<pcl::RGB>::Ptr left_cloud(new pcl::PointCloud<pcl::RGB>);
+ pcl::PointCloud<pcl::RGB>::Ptr right_cloud(new pcl::PointCloud<pcl::RGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud(
+ new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ pcl::PCDReader pcd;
+ pcd.read(left_images[images_idx], *left_cloud);
+ pcd.read(right_images[images_idx], *right_cloud);
+ processStereoPair(left_cloud, right_cloud, out_cloud);
+ processCloud(out_cloud);
+ images_idx++;
+
+ trigger = false;
+ }
+
+ // Draw visualizations
+ if (cloud_mutex.try_lock()) {
+ if (!viewer->updatePointCloud(prev_ground_image, "cloud"))
+ viewer->addPointCloud(prev_ground_image, "cloud");
+
+ if (prev_normal_cloud->points.size() > 1000 && display_normals) {
+ viewer->removePointCloud("normals");
+ viewer->addPointCloudNormals<PointT, pcl::Normal>(
+ prev_ground_image, prev_normal_cloud, 10, 0.15f, "normals");
+ viewer->setPointCloudRenderingProperties(
+ pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "normals");
}
-
- // Draw visualizations
- if (cloud_mutex.try_lock ())
- {
- if (!viewer->updatePointCloud (prev_ground_image, "cloud"))
- viewer->addPointCloud (prev_ground_image, "cloud");
-
- if (prev_normal_cloud->points.size () > 1000 && display_normals)
- {
- viewer->removePointCloud ("normals");
- viewer->addPointCloudNormals<PointT, pcl::Normal>(prev_ground_image, prev_normal_cloud, 10, 0.15f, "normals");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "normals");
- }
-
- if (prev_cloud->points.size () > 1000)
- {
- image_viewer->addRGBImage<PointT>(prev_ground_image, "rgb_image", 0.3);
- }
-
- // Show the groundplane normal
- Eigen::Vector3f nominal_road_normal (0.0, -1.0, 0.0);
- // Adjust for camera tilt
- Eigen::Vector3f tilt_road_normal = Eigen::AngleAxisf (pcl::deg2rad (5.0f), Eigen::Vector3f::UnitX ()) * nominal_road_normal;
-
- // Show the groundplane normal
- pcl::PointXYZ np1 (prev_ground_centroid[0], prev_ground_centroid[1], prev_ground_centroid[2]);
- pcl::PointXYZ np2 (prev_ground_centroid[0] + prev_ground_normal[0],
- prev_ground_centroid[1] + prev_ground_normal[1],
- prev_ground_centroid[2] + prev_ground_normal[2]);
- pcl::PointXYZ np3 (prev_ground_centroid[0] + tilt_road_normal[0],
- prev_ground_centroid[1] + tilt_road_normal[1],
- prev_ground_centroid[2] + tilt_road_normal[2]);
-
- viewer->removeShape ("ground_norm");
- viewer->addArrow (np2, np1, 1.0, 0, 0, false, "ground_norm");
- viewer->removeShape ("expected_ground_norm");
- viewer->addArrow (np3, np1, 0.0, 1.0, 0, false, "expected_ground_norm");
-
- cloud_mutex.unlock ();
+
+ if (prev_cloud->points.size() > 1000) {
+ image_viewer->addRGBImage<PointT>(prev_ground_image, "rgb_image", 0.3);
}
- viewer->spinOnce (100);
- image_viewer->spinOnce (100);
-
+
+ // Show the groundplane normal
+ Eigen::Vector3f nominal_road_normal(0.0, -1.0, 0.0);
+ // Adjust for camera tilt
+ Eigen::Vector3f tilt_road_normal =
+ Eigen::AngleAxisf(pcl::deg2rad(5.0f), Eigen::Vector3f::UnitX()) *
+ nominal_road_normal;
+
+ // Show the groundplane normal
+ pcl::PointXYZ np1(
+ prev_ground_centroid[0], prev_ground_centroid[1], prev_ground_centroid[2]);
+ pcl::PointXYZ np2(prev_ground_centroid[0] + prev_ground_normal[0],
+ prev_ground_centroid[1] + prev_ground_normal[1],
+ prev_ground_centroid[2] + prev_ground_normal[2]);
+ pcl::PointXYZ np3(prev_ground_centroid[0] + tilt_road_normal[0],
+ prev_ground_centroid[1] + tilt_road_normal[1],
+ prev_ground_centroid[2] + tilt_road_normal[2]);
+
+ viewer->removeShape("ground_norm");
+ viewer->addArrow(np2, np1, 1.0, 0, 0, false, "ground_norm");
+ viewer->removeShape("expected_ground_norm");
+ viewer->addArrow(np3, np1, 0.0, 1.0, 0, false, "expected_ground_norm");
+
+ cloud_mutex.unlock();
}
+ viewer->spinOnce(100);
+ image_viewer->spinOnce(100);
}
-
-
-
+ }
};
int
-main (int argc, char** argv)
+main(int argc, char** argv)
{
- if (argc < 3)
- {
- PCL_INFO ("usage: pcl_stereo_ground_segmentation left_image_directory right_image_directory\n");
- PCL_INFO ("note: images must be in PCD format. See pcl_png2pcd\n");
+ if (argc < 3) {
+ PCL_INFO("usage: pcl_stereo_ground_segmentation left_image_directory "
+ "right_image_directory\n");
+ PCL_INFO("note: images must be in PCD format. See pcl_png2pcd\n");
}
// Get list of stereo files
std::vector<std::string> left_images;
boost::filesystem::directory_iterator end_itr;
- for (boost::filesystem::directory_iterator itr (argv[1]); itr != end_itr; ++itr)
- {
- left_images.push_back (itr->path ().string ());
+ for (boost::filesystem::directory_iterator itr(argv[1]); itr != end_itr; ++itr) {
+ left_images.push_back(itr->path().string());
}
- sort (left_images.begin (), left_images.end ());
+ sort(left_images.begin(), left_images.end());
std::vector<std::string> right_images;
- for (boost::filesystem::directory_iterator itr (argv[2]); itr != end_itr; ++itr)
- {
- right_images.push_back (itr->path ().string ());
+ for (boost::filesystem::directory_iterator itr(argv[2]); itr != end_itr; ++itr) {
+ right_images.push_back(itr->path().string());
}
- sort (right_images.begin (), right_images.end ());
+ sort(right_images.begin(), right_images.end());
- PCL_INFO ("Press space to advance to the next frame, or 'c' to enable continuous mode\n");
+ PCL_INFO(
+ "Press space to advance to the next frame, or 'c' to enable continuous mode\n");
// Process and display
- HRCSSegmentation hrcs (left_images, right_images);
- hrcs.run ();
-
+ HRCSSegmentation hrcs(left_images, right_images);
+ hrcs.run();
+
return 0;
}
-#include <pcl/surface/surfel_smoothing.h>
-#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
-
+#include <pcl/io/pcd_io.h>
+#include <pcl/surface/surfel_smoothing.h>
using namespace pcl;
int
-main (int argc, char **argv)
+main(int argc, char** argv)
{
- if (argc != 5)
- {
- PCL_ERROR ("./surfel_smoothing_test normal_search_radius surfel_scale source_cloud destination_cloud\n");
- return (-1);
+ if (argc != 5) {
+ PCL_ERROR("./surfel_smoothing_test normal_search_radius surfel_scale source_cloud "
+ "destination_cloud\n");
+ return -1;
}
- PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
- PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
+ PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());
+ PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
PCDReader reader;
- reader.read (argv[3], *cloud);
- PCL_INFO ("Cloud read: %s\n", argv[3]);
-
- float normal_search_radius = static_cast<float> (atof (argv[1]));
- float surfel_scale = static_cast<float> (atof (argv[2]));
+ reader.read(argv[3], *cloud);
+ PCL_INFO("Cloud read: %s\n", argv[3]);
+ float normal_search_radius = static_cast<float>(atof(argv[1]));
+ float surfel_scale = static_cast<float>(atof(argv[2]));
NormalEstimation<PointXYZ, Normal> normal_estimation;
- normal_estimation.setInputCloud (cloud);
- search::KdTree<PointXYZ>::Ptr search_tree
(new search::KdTree<PointXYZ>);
- normal_estimation.setSearchMethod (search_tree);
- normal_estimation.setRadiusSearch (normal_search_radius);
- normal_estimation.compute (*normals);
-
- SurfelSmoothing<PointXYZ, Normal> surfel_smoothing (surfel_scale);
- surfel_smoothing.setInputCloud (cloud);
- surfel_smoothing.setInputNormals (normals);
- surfel_smoothing.setSearchMethod (search_tree);
+ normal_estimation.setInputCloud(cloud);
+ search::KdTree<PointXYZ>::Ptr search_tree(new search::KdTree<PointXYZ>);
+ normal_estimation.setSearchMethod(search_tree);
+ normal_estimation.setRadiusSearch(normal_search_radius);
+ normal_estimation.compute(*normals);
+
+ SurfelSmoothing<PointXYZ, Normal> surfel_smoothing(surfel_scale);
+ surfel_smoothing.setInputCloud(cloud);
+ surfel_smoothing.setInputNormals(normals);
+ surfel_smoothing.setSearchMethod(search_tree);
PointCloud<PointXYZ>::Ptr output_positions;
PointCloud<Normal>::Ptr output_normals;
- surfel_smoothing.computeSmoothedCloud (output_positions, output_normals);
+ surfel_smoothing.computeSmoothedCloud(output_positions, output_normals);
- PointCloud<PointNormal>::Ptr output_with_normals (new PointCloud<PointNormal> ());
- pcl::concatenateFields (*output_positions, *normals, *output_with_normals);
+ PointCloud<PointNormal>::Ptr output_with_normals(new PointCloud<PointNormal>());
+ pcl::concatenateFields(*output_positions, *normals, *output_with_normals);
- io::savePCDFileASCII (argv[4], *output_with_normals);
+ io::savePCDFileASCII(argv[4], *output_with_normals);
- return (0);
+ return 0;
}
-#include <vector>
-#include <string>
-#include <sstream>
-#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h>
#include <pcl/console/parse.h>
-#include <pcl/search/kdtree.h>
+#include <pcl/io/pcd_io.h>
#include <pcl/search/brute_force.h>
+#include <pcl/search/kdtree.h>
+
+#include <sstream>
+#include <string>
+#include <vector>
using namespace std;
int
-main (int argc, char ** argv)
+main(int argc, char** argv)
{
- if (argc < 2)
- {
- pcl::console::print_info ("Syntax is: %s [-pcd <pcd-file>] (-radius <radius> [-knn <k>] | -knn <k> )\n", argv[0]);
- return (1);
+ if (argc < 2) {
+ pcl::console::print_info(
+ "Syntax is: %s [-pcd <pcd-file>] (-radius <radius> [-knn <k>] | -knn <k> )\n",
+ argv[0]);
+ return 1;
}
string pcd_path;
- bool use_pcd_file = pcl::console::find_switch (argc, argv, "-pcd");
+ bool use_pcd_file = pcl::console::find_switch(argc, argv, "-pcd");
if (use_pcd_file)
- pcl::console::parse (argc, argv, "-pcd", pcd_path);
+ pcl::console::parse(argc, argv, "-pcd", pcd_path);
float radius = -1;
- if (pcl::console::find_switch (argc, argv, "-radius"))
- pcl::console::parse (argc, argv, "-radius", radius);
+ if (pcl::console::find_switch(argc, argv, "-radius"))
+ pcl::console::parse(argc, argv, "-radius", radius);
int k = -1;
- if (pcl::console::find_switch (argc, argv, "-knn"))
- pcl::console::parse (argc, argv, "-knn", k);
+ if (pcl::console::find_switch(argc, argv, "-knn"))
+ pcl::console::parse(argc, argv, "-knn", k);
- if (radius < 0 && k < 0)
- {
- std::cout << "please specify at least one of the options -radius and -knn" << std::endl;
- return (1);
+ if (radius < 0 && k < 0) {
+ std::cout << "please specify at least one of the options -radius and -knn"
+ << std::endl;
+ return 1;
}
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud
(new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if (use_pcd_file)
- pcl::io::loadPCDFile (pcd_path, *cloud);
- else
- {
- cloud->resize (1000000);
- for (std::size_t idx = 0; idx < cloud->size (); ++idx)
- {
- (*cloud)[idx].x = static_cast<float> (rand () / RAND_MAX);
- (*cloud)[idx].y = static_cast<float> (rand () / RAND_MAX);
- (*cloud)[idx].z = static_cast<float> (rand () / RAND_MAX);
+ pcl::io::loadPCDFile(pcd_path, *cloud);
+ else {
+ cloud->resize(1000000);
+ for (std::size_t idx = 0; idx < cloud->size(); ++idx) {
+ (*cloud)[idx].x = static_cast<float>(rand() / RAND_MAX);
+ (*cloud)[idx].y = static_cast<float>(rand() / RAND_MAX);
+ (*cloud)[idx].z = static_cast<float>(rand() / RAND_MAX);
}
}
double bf_setup;
double bf_search;
- if (k > 0)
- {
- start = pcl::getTime ();
- tree.setInputCloud (cloud);
- stop = pcl::getTime ();
+ if (k > 0) {
+ start = pcl::getTime();
+ tree.setInputCloud(cloud);
+ stop = pcl::getTime();
std::cout << "setting up kd tree: " << (kd_setup = stop - start) << std::endl;
- start = pcl::getTime ();
- tree.nearestKSearchT (query, k, kd_indices, kd_distances);
- stop = pcl::getTime ();
- std::cout << "single search with kd tree; " << (kd_search = stop - start) << " :: " << kd_indices[0] << " , " << kd_distances [0] << std::endl;
+ start = pcl::getTime();
+ tree.nearestKSearchT(query, k, kd_indices, kd_distances);
+ stop = pcl::getTime();
+ std::cout << "single search with kd tree; " << (kd_search = stop - start)
+ << " :: " << kd_indices[0] << " , " << kd_distances[0] << std::endl;
pcl::search::BruteForce<pcl::PointXYZRGB> brute_force;
- start = pcl::getTime ();
- brute_force.setInputCloud (cloud);
- stop = pcl::getTime ();
- std::cout << "setting up brute force search: " << (bf_setup = stop - start) << std::endl;
-
- start = pcl::getTime ();
- brute_force.nearestKSearchT (query, k, bf_indices, bf_distances);
- stop = pcl::getTime ();
- std::cout << "single search with brute force; " << (bf_search = stop - start) << " :: " << bf_indices[0] << " , " << bf_distances [0] << std::endl;
- std::cout << "amortization after searches: " << (kd_setup - bf_setup) / (bf_search - kd_search) << std::endl;
+ start = pcl::getTime();
+ brute_force.setInputCloud(cloud);
+ stop = pcl::getTime();
+ std::cout << "setting up brute force search: " << (bf_setup = stop - start)
+ << std::endl;
+
+ start = pcl::getTime();
+ brute_force.nearestKSearchT(query, k, bf_indices, bf_distances);
+ stop = pcl::getTime();
+ std::cout << "single search with brute force; " << (bf_search = stop - start)
+ << " :: " << bf_indices[0] << " , " << bf_distances[0] << std::endl;
+ std::cout << "amortization after searches: "
+ << (kd_setup - bf_setup) / (bf_search - kd_search) << std::endl;
}
- else
- {
- start = pcl::getTime ();
- tree.setInputCloud (cloud);
- stop = pcl::getTime ();
+ else {
+ start = pcl::getTime();
+ tree.setInputCloud(cloud);
+ stop = pcl::getTime();
std::cout << "setting up kd tree: " << (kd_setup = stop - start) << std::endl;
- start = pcl::getTime ();
- tree.radiusSearch (query, radius, kd_indices, kd_distances, k);
- stop = pcl::getTime ();
- std::cout << "single search with kd tree; " << (kd_search = stop - start) << " :: " << kd_indices[0] << " , " << kd_distances [0] << std::endl;
+ start = pcl::getTime();
+ tree.radiusSearch(query, radius, kd_indices, kd_distances, k);
+ stop = pcl::getTime();
+ std::cout << "single search with kd tree; " << (kd_search = stop - start)
+ << " :: " << kd_indices[0] << " , " << kd_distances[0] << std::endl;
pcl::search::BruteForce<pcl::PointXYZRGB> brute_force;
- start = pcl::getTime ();
- brute_force.setInputCloud (cloud);
- stop = pcl::getTime ();
- std::cout << "setting up brute force search: " << (bf_setup = stop - start) << std::endl;
-
- start = pcl::getTime ();
- brute_force.radiusSearch (query, radius, bf_indices, bf_distances, k);
- stop = pcl::getTime ();
- std::cout << "single search with brute force; " << (bf_search = stop - start) << " :: " << bf_indices[0] << " , " << bf_distances [0] << std::endl;
- std::cout << "amortization after searches: " << (kd_setup - bf_setup) / (bf_search - kd_search) << std::endl;
+ start = pcl::getTime();
+ brute_force.setInputCloud(cloud);
+ stop = pcl::getTime();
+ std::cout << "setting up brute force search: " << (bf_setup = stop - start)
+ << std::endl;
+
+ start = pcl::getTime();
+ brute_force.radiusSearch(query, radius, bf_indices, bf_distances, k);
+ stop = pcl::getTime();
+ std::cout << "single search with brute force; " << (bf_search = stop - start)
+ << " :: " << bf_indices[0] << " , " << bf_distances[0] << std::endl;
+ std::cout << "amortization after searches: "
+ << (kd_setup - bf_setup) / (bf_search - kd_search) << std::endl;
}
- if (kd_indices.size () != bf_indices.size ())
- {
- std::cerr << "size of results do not match " <<kd_indices.size () << " vs. " << bf_indices.size () << std::endl;
+ if (kd_indices.size() != bf_indices.size()) {
+ std::cerr << "size of results do not match " << kd_indices.size() << " vs. "
+ << bf_indices.size() << std::endl;
}
- else
- {
- std::cerr << "size of result: " <<kd_indices.size () << std::endl;
- for (std::size_t idx = 0; idx < kd_indices.size (); ++idx)
- {
- if (kd_indices[idx] != bf_indices[idx] && kd_distances[idx] != bf_distances[idx])
- {
- std::cerr << "results do not match: " << idx << " nearest neighbor: "
- << kd_indices[idx] << " with distance: " << kd_distances[idx] << " vs. "
- << bf_indices[idx] << " with distance: " << bf_distances[idx] << std::endl;
+ else {
+ std::cerr << "size of result: " << kd_indices.size() << std::endl;
+ for (std::size_t idx = 0; idx < kd_indices.size(); ++idx) {
+ if (kd_indices[idx] != bf_indices[idx] &&
+ kd_distances[idx] != bf_distances[idx]) {
+ std::cerr << "results do not match: " << idx
+ << " nearest neighbor: " << kd_indices[idx]
+ << " with distance: " << kd_distances[idx] << " vs. "
+ << bf_indices[idx] << " with distance: " << bf_distances[idx]
+ << std::endl;
}
}
}
- return (0);
+ return 0;
}
#
# .. code-block:: cmake
#
-# find_package(ClangFormat)
+# find_package(ClangFormat)
# if(ClangFormat_FOUND)
# message("clang-format executable found: ${ClangFormat_EXECUTABLE}\n"
-# "version: ${ClangFormat_VERSION}")
+# "version: ${ClangFormat_VERSION}")
# endif()
find_program(ClangFormat_EXECUTABLE
NAMES
+ # unreleased versions
+ clang-format-14
+ clang-format-13
+ clang-format-12
+ clang-format-11
+ # current latest
+ clang-format-10
clang-format-9
- clang-format-9.0
+ # since clang-format-8, only major version is prefixed
clang-format-8
clang-format-8.0
clang-format-7
PATHS /usr/include /usr/local/include /opt/local/include /sw/include
PATH_SUFFIXES libusb-1.0)
+ # We need to look for libusb-1.0 too because find_library does not attempt to find
+ # library files with a "lib" prefix implicitly on Windows
find_library(LIBUSB_1_LIBRARY
- NAMES usb-1.0
+ NAMES usb-1.0 libusb-1.0
PATHS /usr/lib /usr/local/lib /opt/local/lib /sw/lib)
set(LIBUSB_1_INCLUDE_DIRS ${LIBUSB_1_INCLUDE_DIR})
-find_package(ClangFormat 7)
+find_package(ClangFormat 10)
# search for version number in clang-format without version number
if(ClangFormat_FOUND)
message(STATUS "Adding target 'format'")
add_custom_target(
format
- COMMAND sh
+ COMMAND sh
${PCL_SOURCE_DIR}/.dev/format.sh
${ClangFormat_EXECUTABLE}
${PCL_SOURCE_DIR}
target_compile_features(${_name} PUBLIC ${PCL_CXX_COMPILE_FEATURES})
# must link explicitly against boost.
target_link_libraries(${_name} ${Boost_LIBRARIES} Threads::Threads)
+ if(TARGET OpenMP::OpenMP_CXX)
+ target_link_libraries(${_name} OpenMP::OpenMP_CXX)
+ endif()
+
if((UNIX AND NOT ANDROID) OR MINGW)
target_link_libraries(${_name} m)
endif()
target_link_libraries(${_exename} ${Boost_LIBRARIES})
#Only applies to MSVC
- if(MSVC)
+ if(MSVC)
#Requires CMAKE version 3.13.0
if(CMAKE_VERSION VERSION_LESS "3.13.0" AND (NOT ArgumentWarningShown))
message(WARNING "Arguments for unit test projects are not added - this requires at least CMake 3.13. Can be added manually in \"Project settings -> Debugging -> Command arguments\"")
set(oneValueArgs COMPONENT DESC CFLAGS LIB_FLAGS)
set(multiValueArgs PCL_DEPS INT_DEPS EXT_DEPS)
cmake_parse_arguments(PKGCONFIG "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
-
+
set(PKG_NAME ${_name})
set(PKG_DESC ${PKGCONFIG_DESC})
set(PKG_CFLAGS ${PKGCONFIG_CFLAGS})
if(${ARGC} EQUAL 3)
set(_reason ${ARGV2})
else()
- set(_reason "No reason")
+ set(_reason "No reason provided")
endif()
SET_IN_GLOBAL_MAP(PCL_SUBSYS_STATUS ${_name} ${_status})
SET_IN_GLOBAL_MAP(PCL_SUBSYS_REASONS ${_name} ${_reason})
if(${ARGC} EQUAL 4)
set(_reason ${ARGV2})
else()
- set(_reason "No reason")
+ set(_reason "No reason provided")
endif()
SET_IN_GLOBAL_MAP(PCL_SUBSYS_STATUS ${_parent}_${_name} ${_status})
SET_IN_GLOBAL_MAP(PCL_SUBSYS_REASONS ${_parent}_${_name} ${_reason})
set(incs
include/pcl/correspondence.h
+ include/pcl/memory.h
include/pcl/exceptions.h
include/pcl/pcl_base.h
include/pcl/pcl_exports.h
include/pcl/pcl_macros.h
+ include/pcl/types.h
include/pcl/point_cloud.h
include/pcl/point_traits.h
+ include/pcl/type_traits.h
include/pcl/point_types_conversion.h
include/pcl/point_representation.h
include/pcl/point_types.h
include/pcl/common/utils.h
include/pcl/common/geometry.h
include/pcl/common/gaussian.h
- include/pcl/common/point_operators.h
include/pcl/common/spring.h
include/pcl/common/intensity.h
include/pcl/common/random.h
set(LIB_NAME "pcl_${SUBSYS_NAME}")
include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
PCL_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${kissfft_srcs} ${incs} ${common_incs} ${impl_incs} ${tools_incs} ${kissfft_incs} ${common_incs_impl} ${range_image_incs} ${range_image_incs_impl})
+
+if(MSVC AND NOT (MSVC_VERSION LESS 1915))
+ # MSVC resolved a byte alignment issue in compiler version 15.9
+ # We get this due to using Eigen objects and allocating those objects with make_shared
+ # It has to be public so that everything linking with common has this definition too
+ target_compile_definitions(${LIB_NAME} PUBLIC _ENABLE_EXTENDED_ALIGNED_STORAGE)
+endif()
+
PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC})
# Install include files
#pragma once
+#include <pcl/memory.h> // for shared_ptr
+
#include <string> // for string
#include <ostream> // for ostream
-#include <pcl/make_shared.h> // for shared_ptr
-
namespace pcl
{
struct PCLHeader
}
} // namespace pcl
+
#include <boost/predef/other/endian.h>
+#include <pcl/pcl_macros.h> // for PCL_EXPORTS
#include <pcl/PCLHeader.h>
#include <pcl/PCLPointField.h>
#pragma once
+#include <pcl/memory.h> // for shared_ptr
+#include <pcl/type_traits.h> // for asEnum_v
+
#include <string> // for string
#include <ostream> // for ostream
-#include <pcl/pcl_macros.h> // for shared_ptr
-
namespace pcl
{
struct PCLPointField
std::uint8_t datatype = 0;
std::uint32_t count = 0;
- enum PointFieldTypes { INT8 = 1,
- UINT8 = 2,
- INT16 = 3,
- UINT16 = 4,
- INT32 = 5,
- UINT32 = 6,
- FLOAT32 = 7,
- FLOAT64 = 8 };
+ enum PointFieldTypes { INT8 = traits::asEnum_v<std::int8_t>,
+ UINT8 = traits::asEnum_v<std::uint8_t>,
+ INT16 = traits::asEnum_v<std::int16_t>,
+ UINT16 = traits::asEnum_v<std::uint16_t>,
+ INT32 = traits::asEnum_v<std::int32_t>,
+ UINT32 = traits::asEnum_v<std::uint32_t>,
+ FLOAT32 = traits::asEnum_v<float>,
+ FLOAT64 = traits::asEnum_v<double>};
public:
using Ptr = shared_ptr< ::pcl::PCLPointField>;
s << " " << v.count << std::endl;
return (s);
}
+
+ // Return true if the PCLPointField matches the expected name and data type.
+ // Written as a struct to allow partially specializing on Tag.
+ template<typename PointT, typename Tag>
+ struct FieldMatches
+ {
+ bool operator() (const PCLPointField& field)
+ {
+ return ((field.name == traits::name<PointT, Tag>::value) &&
+ (field.datatype == traits::datatype<PointT, Tag>::value) &&
+ ((field.count == traits::datatype<PointT, Tag>::size) ||
+ (field.count == 0 && traits::datatype<PointT, Tag>::size == 1 /* see bug #821 */)));
+ }
+ };
+
} // namespace pcl
+
// Include the correct Header path here
#include <pcl/PCLHeader.h>
+#include <pcl/types.h>
namespace pcl
{
struct PointIndices
{
+ using Ptr = shared_ptr< ::pcl::PointIndices>;
+ using ConstPtr = shared_ptr<const ::pcl::PointIndices>;
+
PointIndices ()
{}
::pcl::PCLHeader header;
- std::vector<int> indices;
-
- public:
- using Ptr = shared_ptr< ::pcl::PointIndices>;
- using ConstPtr = shared_ptr<const ::pcl::PointIndices>;
+ Indices indices;
}; // struct PointIndices
using PointIndicesPtr = PointIndices::Ptr;
#pragma once
+#include <pcl/memory.h>
+#include <pcl/pcl_macros.h>
+
#include <string>
#include <vector>
#include <ostream>
-#include <boost/shared_ptr.hpp>
-#include <pcl/pcl_macros.h>
namespace pcl
{
#ifndef Q_MOC_RUN
// Marking all Boost headers as system headers to remove warnings
#include <boost/fusion/sequence/intrinsic/at_key.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/make_shared.hpp>
#include <boost/mpl/size.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/signals2.hpp>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
#include <pcl/PointIndices.h>
#include <pcl/cloud_iterator.h>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/filter_if.hpp>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
* $Id$
*
*/
-#ifndef BIVARIATE_POLYNOMIAL_HPP
-#define BIVARIATE_POLYNOMIAL_HPP
+
+#pragma once
+
+#include <pcl/common/bivariate_polynomial.h>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template<typename real>
-pcl::BivariatePolynomialT<real>::BivariatePolynomialT (int new_degree) :
+BivariatePolynomialT<real>::BivariatePolynomialT (int new_degree) :
degree(0), parameters(nullptr), gradient_x(nullptr), gradient_y(nullptr)
{
setDegree(new_degree);
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real>
-pcl::BivariatePolynomialT<real>::BivariatePolynomialT (const BivariatePolynomialT& other) :
+BivariatePolynomialT<real>::BivariatePolynomialT (const BivariatePolynomialT& other) :
degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
{
deepCopy (other);
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real>
-pcl::BivariatePolynomialT<real>::~BivariatePolynomialT ()
+BivariatePolynomialT<real>::~BivariatePolynomialT ()
{
memoryCleanUp ();
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::setDegree (int newDegree)
+BivariatePolynomialT<real>::setDegree (int newDegree)
{
if (newDegree <= 0)
{
delete gradient_y; gradient_y = nullptr;
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::memoryCleanUp ()
+BivariatePolynomialT<real>::memoryCleanUp ()
{
delete[] parameters; parameters = nullptr;
delete gradient_x; gradient_x = nullptr;
delete gradient_y; gradient_y = nullptr;
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::deepCopy (const pcl::BivariatePolynomialT<real>& other)
+BivariatePolynomialT<real>::deepCopy (const pcl::BivariatePolynomialT<real>& other)
{
if (this == &other) return;
if (degree != other.degree)
degree = other.degree;
parameters = new real[getNoOfParameters ()];
}
- if (other.gradient_x == NULL)
+ if (!other.gradient_x)
{
- delete gradient_x; gradient_x=NULL;
- delete gradient_y; gradient_y=NULL;
+ delete gradient_x;
+ delete gradient_y;
+ gradient_x = nullptr;
+ gradient_y = nullptr;
}
- else if (gradient_x==NULL)
+ else if (!gradient_x)
{
gradient_x = new pcl::BivariatePolynomialT<real> ();
gradient_y = new pcl::BivariatePolynomialT<real> ();
std::copy_n(other.parameters, getNoOfParameters (), parameters);
- if (other.gradient_x != NULL)
+ if (other.gradient_x != nullptr)
{
gradient_x->deepCopy (*other.gradient_x);
gradient_y->deepCopy (*other.gradient_y);
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::calculateGradient (bool forceRecalc)
+BivariatePolynomialT<real>::calculateGradient (bool forceRecalc)
{
if (gradient_x!=NULL && !forceRecalc) return;
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> real
-pcl::BivariatePolynomialT<real>::getValue (real x, real y) const
+BivariatePolynomialT<real>::getValue (real x, real y) const
{
unsigned int parametersSize = getNoOfParameters ();
real* tmpParameter = ¶meters[parametersSize-1];
return ret;
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY)
+BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY)
{
calculateGradient ();
gradX = gradient_x->getValue (x, y);
gradY = gradient_y->getValue (x, y);
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
- std::vector<int>& types) const
+BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
+ std::vector<int>& types) const
{
x_values.clear ();
y_values.clear ();
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> std::ostream&
-pcl::operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p)
+operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p)
{
real* tmpParameter = p.parameters;
bool first = true;
return (os);
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::writeBinary (std::ostream& os) const
+BivariatePolynomialT<real>::writeBinary (std::ostream& os) const
{
os.write (reinterpret_cast<const char*> (°ree), sizeof (int));
unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
os.write (reinterpret_cast<const char*> (this->parameters), paramCnt * sizeof (real));
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::writeBinary (const char* filename) const
+BivariatePolynomialT<real>::writeBinary (const char* filename) const
{
std::ofstream fout (filename);
writeBinary (fout);
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::readBinary (std::istream& os)
+BivariatePolynomialT<real>::readBinary (std::istream& os)
{
memoryCleanUp ();
os.read (reinterpret_cast<char*> (&this->degree), sizeof (int));
os.read (reinterpret_cast<char*> (&(*this->parameters)), paramCnt * sizeof (real));
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename real> void
-pcl::BivariatePolynomialT<real>::readBinary (const char* filename)
+BivariatePolynomialT<real>::readBinary (const char* filename)
{
std::ifstream fin (filename);
readBinary (fin);
}
-#endif
+} // namespace pcl
*
*/
-#ifndef PCL_COMMON_IMPL_CENTROID_H_
-#define PCL_COMMON_IMPL_CENTROID_H_
+#pragma once
#include <pcl/common/centroid.h>
#include <pcl/conversions.h>
-#include <boost/mpl/size.hpp>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
+#include <boost/fusion/algorithm/transformation/filter_if.hpp> // for boost::fusion::filter_if
+#include <boost/fusion/algorithm/iteration/for_each.hpp> // for boost::fusion::for_each
+#include <boost/mpl/size.hpp> // for boost::mpl::size
+
+
+namespace pcl
+{
-///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
- Eigen::Matrix<Scalar, 4, 1> ¢roid)
+compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
+ Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
// Initialize to 0
centroid.setZero ();
-
+
unsigned cp = 0;
// For each point in the cloud
return (cp);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
- Eigen::Matrix<Scalar, 4, 1> ¢roid)
+compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
+ Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
if (cloud.empty ())
return (0);
return (cp);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
- const std::vector<int> &indices,
- Eigen::Matrix<Scalar, 4, 1> ¢roid)
+compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
+ const std::vector<int> &indices,
+ Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
if (indices.empty ())
return (0);
return (cp);
}
-/////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
- const pcl::PointIndices &indices,
- Eigen::Matrix<Scalar, 4, 1> ¢roid)
+compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
+ const pcl::PointIndices &indices,
+ Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
return (pcl::compute3DCentroid (cloud, indices.indices, centroid));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned
-
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
if (cloud.empty ())
return (0);
return (point_count);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
if (point_count != 0)
return (point_count);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- const std::vector<int> &indices,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ const std::vector<int> &indices,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
if (indices.empty ())
return (0);
return (static_cast<unsigned int> (point_count));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- const pcl::PointIndices &indices,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ const pcl::PointIndices &indices,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
- const std::vector<int> &indices,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
+ const std::vector<int> &indices,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
if (point_count != 0)
return (point_count);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
- const pcl::PointIndices &indices,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
+ const pcl::PointIndices &indices,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
unsigned int point_count = pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix);
if (point_count != 0)
return point_count;
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
return (point_count);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- const std::vector<int> &indices,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ const std::vector<int> &indices,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
return (point_count);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- const pcl::PointIndices &indices,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
+computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ const pcl::PointIndices &indices,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
- Eigen::Matrix<Scalar, 4, 1> ¢roid)
+computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
+ Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
return (static_cast<unsigned int> (point_count));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- const std::vector<int> &indices,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
- Eigen::Matrix<Scalar, 4, 1> ¢roid)
+computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ const std::vector<int> &indices,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
+ Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
return (static_cast<unsigned int> (point_count));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline unsigned int
-
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
- const pcl::PointIndices &indices,
- Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
- Eigen::Matrix<Scalar, 4, 1> ¢roid)
+computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
+ const pcl::PointIndices &indices,
+ Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
+ Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
-
pcl::PointCloud<PointT> &cloud_out,
- int npts)
+demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ pcl::PointCloud<PointT> &cloud_out,
+ int npts)
{
// Calculate the number of points if not given
if (npts == 0)
cloud_out.height = 1;
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
-
pcl::PointCloud<PointT> &cloud_out)
+demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ pcl::PointCloud<PointT> &cloud_out)
{
cloud_out = cloud_in;
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const std::vector<int> &indices,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
-
pcl::PointCloud<PointT> &cloud_out)
+demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<int> &indices,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ pcl::PointCloud<PointT> &cloud_out)
{
cloud_out.header = cloud_in.header;
cloud_out.is_dense = cloud_in.is_dense;
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const pcl::PointIndices& indices,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
-
pcl::PointCloud<PointT> &cloud_out)
+demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const pcl::PointIndices& indices,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ pcl::PointCloud<PointT> &cloud_out)
{
return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
- int npts)
+demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
+ int npts)
{
// Calculate the number of points if not given
if (npts == 0)
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
+demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
{
std::size_t npts = cloud_in.size ();
//cloud_out.block (3, 0, 1, npts).setZero ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const std::vector<int> &indices,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
+demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<int> &indices,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
{
std::size_t npts = indices.size ();
//cloud_out.block (3, 0, 1, npts).setZero ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const pcl::PointIndices &indices,
- const Eigen::Matrix<Scalar, 4, 1> ¢roid,
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
+demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const pcl::PointIndices &indices,
+ const Eigen::Matrix<Scalar, 4, 1> ¢roid,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
{
return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline void
-pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
- Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
+computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
{
using FieldList = typename pcl::traits::fieldList<PointT>::type;
centroid /= static_cast<Scalar> (cloud.size ());
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline void
-pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
- const std::vector<int> &indices,
- Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
+computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
+ const std::vector<int> &indices,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
{
using FieldList = typename pcl::traits::fieldList<PointT>::type;
centroid /= static_cast<Scalar> (indices.size ());
}
-/////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline void
-
pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
- const pcl::PointIndices &indices,
- Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
+computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
+ const pcl::PointIndices &indices,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
{
return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
}
template <typename PointT> void
-
pcl::CentroidPoint<PointT>::add (const PointT& point)
+CentroidPoint<PointT>::add (const PointT& point)
{
// Invoke add point on each accumulator
boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
template <typename PointT>
template <typename PointOutT> void
-
pcl::CentroidPoint<PointT>::get (PointOutT& point) const
+CentroidPoint<PointT>::get (PointOutT& point) const
{
if (num_points_ != 0)
{
}
}
+
template <typename PointInT, typename PointOutT> std::size_t
-
pcl::computeCentroid (const pcl::PointCloud<PointInT>& cloud,
+computeCentroid (const pcl::PointCloud<PointInT>& cloud,
PointOutT& centroid)
{
pcl::CentroidPoint<PointInT> cp;
return (cp.getSize ());
}
+
template <typename PointInT, typename PointOutT> std::size_t
-
pcl::computeCentroid (const pcl::PointCloud<PointInT>& cloud,
+computeCentroid (const pcl::PointCloud<PointInT>& cloud,
const std::vector<int>& indices,
PointOutT& centroid)
{
return (cp.getSize ());
}
-#endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_
+} // namespace pcl
*
*/
-#ifndef PCL_COMMON_IMPL_COPY_POINT_HPP_
-#define PCL_COMMON_IMPL_COPY_POINT_HPP_
+#pragma once
#include <pcl/point_types.h>
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
#include <pcl/for_each_type.h>
#include <pcl/common/concatenate.h>
+#include <pcl/common/copy_point.h>
+
namespace pcl
{
- namespace detail
- {
-
- /* CopyPointHelper and its specializations copy the contents of a source
- * point to a target point. There are three cases:
- *
- * - Points have the same type.
- * In this case a single `memcpy` is used.
- *
- * - Points have different types and one of the following is true:
- * * both have RGB fields;
- * * both have RGBA fields;
- * * one or both have no RGB/RGBA fields.
- * In this case we find the list of common fields and copy their
- * contents one by one with `NdConcatenateFunctor`.
- *
- * - Points have different types and one of these types has RGB field, and
- * the other has RGBA field.
- * In this case we also find the list of common fields and copy their
- * contents. In order to account for the fact that RGB and RGBA do not
- * match we have an additional `memcpy` to copy the contents of one into
- * another.
- *
- * An appropriate version of CopyPointHelper is instantiated during
- * compilation time automatically, so there is absolutely no run-time
- * overhead. */
+namespace detail
+{
- template <typename PointInT, typename PointOutT, typename Enable = void>
- struct CopyPointHelper { };
+/* CopyPointHelper and its specializations copy the contents of a source
+ * point to a target point. There are three cases:
+ *
+ * - Points have the same type.
+ * In this case a single `memcpy` is used.
+ *
+ * - Points have different types and one of the following is true:
+ * * both have RGB fields;
+ * * both have RGBA fields;
+ * * one or both have no RGB/RGBA fields.
+ * In this case we find the list of common fields and copy their
+ * contents one by one with `NdConcatenateFunctor`.
+ *
+ * - Points have different types and one of these types has RGB field, and
+ * the other has RGBA field.
+ * In this case we also find the list of common fields and copy their
+ * contents. In order to account for the fact that RGB and RGBA do not
+ * match we have an additional `memcpy` to copy the contents of one into
+ * another.
+ *
+ * An appropriate version of CopyPointHelper is instantiated during
+ * compilation time automatically, so there is absolutely no run-time
+ * overhead. */
- template <typename PointInT, typename PointOutT>
- struct CopyPointHelper<PointInT, PointOutT, std::enable_if_t<std::is_same<PointInT, PointOutT>::value>>
- {
- void operator () (const PointInT& point_in, PointOutT& point_out) const
- {
- memcpy (&point_out, &point_in, sizeof (PointInT));
- }
- };
+template <typename PointInT, typename PointOutT, typename Enable = void>
+struct CopyPointHelper { };
- template <typename PointInT, typename PointOutT>
- struct CopyPointHelper<PointInT, PointOutT,
- std::enable_if_t<boost::mpl::and_<boost::mpl::not_<std::is_same<PointInT, PointOutT>>,
- boost::mpl::or_<boost::mpl::not_<pcl::traits::has_color<PointInT>>,
- boost::mpl::not_<pcl::traits::has_color<PointOutT>>,
- boost::mpl::and_<pcl::traits::has_field<PointInT, pcl::fields::rgb>,
- pcl::traits::has_field<PointOutT, pcl::fields::rgb>>,
- boost::mpl::and_<pcl::traits::has_field<PointInT, pcl::fields::rgba>,
- pcl::traits::has_field<PointOutT, pcl::fields::rgba>>>>::value>>
- {
- void operator () (const PointInT& point_in, PointOutT& point_out) const
- {
- using FieldListInT = typename pcl::traits::fieldList<PointInT>::type;
- using FieldListOutT = typename pcl::traits::fieldList<PointOutT>::type;
- using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
- pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (point_in, point_out));
- }
- };
+template <typename PointInT, typename PointOutT>
+struct CopyPointHelper<PointInT, PointOutT, std::enable_if_t<std::is_same<PointInT, PointOutT>::value>>
+{
+ void operator () (const PointInT& point_in, PointOutT& point_out) const
+ {
+ memcpy (&point_out, &point_in, sizeof (PointInT));
+ }
+};
- template <typename PointInT, typename PointOutT>
- struct CopyPointHelper<PointInT, PointOutT,
- std::enable_if_t<boost::mpl::and_<boost::mpl::not_<std::is_same<PointInT, PointOutT>>,
- boost::mpl::or_<boost::mpl::and_<pcl::traits::has_field<PointInT, pcl::fields::rgb>,
- pcl::traits::has_field<PointOutT, pcl::fields::rgba>>,
- boost::mpl::and_<pcl::traits::has_field<PointInT, pcl::fields::rgba>,
- pcl::traits::has_field<PointOutT, pcl::fields::rgb>>>>::value>>
- {
- void operator () (const PointInT& point_in, PointOutT& point_out) const
- {
- using FieldListInT = typename pcl::traits::fieldList<PointInT>::type;
- using FieldListOutT = typename pcl::traits::fieldList<PointOutT>::type;
- using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
- const std::uint32_t offset_in = boost::mpl::if_<pcl::traits::has_field<PointInT, pcl::fields::rgb>,
- pcl::traits::offset<PointInT, pcl::fields::rgb>,
- pcl::traits::offset<PointInT, pcl::fields::rgba> >::type::value;
- const std::uint32_t offset_out = boost::mpl::if_<pcl::traits::has_field<PointOutT, pcl::fields::rgb>,
- pcl::traits::offset<PointOutT, pcl::fields::rgb>,
- pcl::traits::offset<PointOutT, pcl::fields::rgba> >::type::value;
- pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (point_in, point_out));
- memcpy (reinterpret_cast<char*> (&point_out) + offset_out,
- reinterpret_cast<const char*> (&point_in) + offset_in,
- 4);
- }
- };
+template <typename PointInT, typename PointOutT>
+struct CopyPointHelper<PointInT, PointOutT,
+ std::enable_if_t<boost::mpl::and_<boost::mpl::not_<std::is_same<PointInT, PointOutT>>,
+ boost::mpl::or_<boost::mpl::not_<pcl::traits::has_color<PointInT>>,
+ boost::mpl::not_<pcl::traits::has_color<PointOutT>>,
+ boost::mpl::and_<pcl::traits::has_field<PointInT, pcl::fields::rgb>,
+ pcl::traits::has_field<PointOutT, pcl::fields::rgb>>,
+ boost::mpl::and_<pcl::traits::has_field<PointInT, pcl::fields::rgba>,
+ pcl::traits::has_field<PointOutT, pcl::fields::rgba>>>>::value>>
+{
+ void operator () (const PointInT& point_in, PointOutT& point_out) const
+ {
+ using FieldListInT = typename pcl::traits::fieldList<PointInT>::type;
+ using FieldListOutT = typename pcl::traits::fieldList<PointOutT>::type;
+ using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
+ pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (point_in, point_out));
+ }
+};
+template <typename PointInT, typename PointOutT>
+struct CopyPointHelper<PointInT, PointOutT,
+ std::enable_if_t<boost::mpl::and_<boost::mpl::not_<std::is_same<PointInT, PointOutT>>,
+ boost::mpl::or_<boost::mpl::and_<pcl::traits::has_field<PointInT, pcl::fields::rgb>,
+ pcl::traits::has_field<PointOutT, pcl::fields::rgba>>,
+ boost::mpl::and_<pcl::traits::has_field<PointInT, pcl::fields::rgba>,
+ pcl::traits::has_field<PointOutT, pcl::fields::rgb>>>>::value>>
+{
+ void operator () (const PointInT& point_in, PointOutT& point_out) const
+ {
+ using FieldListInT = typename pcl::traits::fieldList<PointInT>::type;
+ using FieldListOutT = typename pcl::traits::fieldList<PointOutT>::type;
+ using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
+ const std::uint32_t offset_in = boost::mpl::if_<pcl::traits::has_field<PointInT, pcl::fields::rgb>,
+ pcl::traits::offset<PointInT, pcl::fields::rgb>,
+ pcl::traits::offset<PointInT, pcl::fields::rgba> >::type::value;
+ const std::uint32_t offset_out = boost::mpl::if_<pcl::traits::has_field<PointOutT, pcl::fields::rgb>,
+ pcl::traits::offset<PointOutT, pcl::fields::rgb>,
+ pcl::traits::offset<PointOutT, pcl::fields::rgba> >::type::value;
+ pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (point_in, point_out));
+ memcpy (reinterpret_cast<char*> (&point_out) + offset_out,
+ reinterpret_cast<const char*> (&point_in) + offset_in,
+ 4);
}
+};
-}
+} // namespace detail
template <typename PointInT, typename PointOutT> void
-pcl::copyPoint (const PointInT& point_in, PointOutT& point_out)
+copyPoint (const PointInT& point_in, PointOutT& point_out)
{
detail::CopyPointHelper<PointInT, PointOutT> copy;
copy (point_in, point_out);
}
-#endif //PCL_COMMON_IMPL_COPY_POINT_HPP_
+} // namespace pcl
*
*/
-#ifndef PCL_COMMON_EIGEN_IMPL_HPP_
-#define PCL_COMMON_EIGEN_IMPL_HPP_
+#pragma once
+
+#include <pcl/common/eigen.h>
+#include <pcl/console/print.h>
#include <array>
#include <algorithm>
#include <cmath>
-#include <pcl/console/print.h>
-//////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename Scalar, typename Roots> inline void
-pcl::computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots)
+computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots)
{
roots (0) = Scalar (0);
Scalar d = Scalar (b * b - 4.0 * c);
roots (1) = 0.5f * (b - sd);
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix, typename Roots> inline void
-pcl::computeRoots (const Matrix& m, Roots& roots)
+computeRoots (const Matrix& m, Roots& roots)
{
using Scalar = typename Matrix::Scalar;
}
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix, typename Vector> inline void
-pcl::eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
+eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
{
// if diagonal matrix, the eigenvalues are the diagonal elements
// and the eigenvectors are not unique, thus set to Identity
eigenvector.normalize ();
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix, typename Vector> inline void
-pcl::eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
+eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
{
// if diagonal matrix, the eigenvalues are the diagonal elements
// and the eigenvectors are not unique, thus set to Identity
eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0);
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix, typename Vector> inline void
-pcl::computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
+computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
{
using Scalar = typename Matrix::Scalar;
// Scale the matrix so its entries are in [-1,1]. The scaling is applied
eigenvector = vec3 / std::sqrt (len3);
}
-namespace pcl {
-namespace detail{
+namespace detail
+{
+
template <typename Vector, typename Scalar>
struct EigenVector {
Vector vector;
return EigenVector<Vector, Scalar> {crossProduct.row (index) / length,
length};
}
+
} // namespace detail
-} // namespace pcl
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix, typename Vector> inline void
-pcl::eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
+eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
{
using Scalar = typename Matrix::Scalar;
// Scale the matrix so its entries are in [-1,1]. The scaling is applied
eigenvector = detail::getLargest3x3Eigenvector<Vector> (scaledMat).vector;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix, typename Vector> inline void
-pcl::eigen33 (const Matrix& mat, Vector& evals)
+eigen33 (const Matrix& mat, Vector& evals)
{
using Scalar = typename Matrix::Scalar;
Scalar scale = mat.cwiseAbs ().maxCoeff ();
evals *= scale;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix, typename Vector> inline void
-pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
+eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
{
using Scalar = typename Matrix::Scalar;
// Scale the matrix so its entries are in [-1,1]. The scaling is applied
evals *= scale;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix> inline typename Matrix::Scalar
-pcl::invert2x2 (const Matrix& matrix, Matrix& inverse)
+invert2x2 (const Matrix& matrix, Matrix& inverse)
{
using Scalar = typename Matrix::Scalar;
Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2);
return det;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix> inline typename Matrix::Scalar
-pcl::invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse)
+invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse)
{
using Scalar = typename Matrix::Scalar;
// elements
return det;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix> inline typename Matrix::Scalar
-pcl::invert3x3Matrix (const Matrix& matrix, Matrix& inverse)
+invert3x3Matrix (const Matrix& matrix, Matrix& inverse)
{
using Scalar = typename Matrix::Scalar;
return det;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Matrix> inline typename Matrix::Scalar
-pcl::determinant3x3Matrix (const Matrix& matrix)
+determinant3x3Matrix (const Matrix& matrix)
{
// result is independent of Row/Col Major storage!
return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) +
matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
void
-pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
- const Eigen::Vector3f& y_direction,
- Eigen::Affine3f& transformation)
+getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
+ const Eigen::Vector3f& y_direction,
+ Eigen::Affine3f& transformation)
{
Eigen::Vector3f tmp0 = (y_direction.cross(z_axis)).normalized();
Eigen::Vector3f tmp1 = (z_axis.cross(tmp0)).normalized();
transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
Eigen::Affine3f
-pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
- const Eigen::Vector3f& y_direction)
+getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
+ const Eigen::Vector3f& y_direction)
{
Eigen::Affine3f transformation;
getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
return (transformation);
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
void
-pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
- const Eigen::Vector3f& y_direction,
- Eigen::Affine3f& transformation)
+getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
+ const Eigen::Vector3f& y_direction,
+ Eigen::Affine3f& transformation)
{
Eigen::Vector3f tmp2 = (x_axis.cross(y_direction)).normalized();
Eigen::Vector3f tmp1 = (tmp2.cross(x_axis)).normalized();
transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
Eigen::Affine3f
-pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
- const Eigen::Vector3f& y_direction)
+getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
+ const Eigen::Vector3f& y_direction)
{
Eigen::Affine3f transformation;
getTransFromUnitVectorsXY (x_axis, y_direction, transformation);
return (transformation);
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
void
-pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
- const Eigen::Vector3f& z_axis,
- Eigen::Affine3f& transformation)
+getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
+ const Eigen::Vector3f& z_axis,
+ Eigen::Affine3f& transformation)
{
getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
Eigen::Affine3f
-pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
- const Eigen::Vector3f& z_axis)
+getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
+ const Eigen::Vector3f& z_axis)
{
Eigen::Affine3f transformation;
getTransformationFromTwoUnitVectors (y_direction, z_axis, transformation);
return (transformation);
}
+
void
-pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
- const Eigen::Vector3f& z_axis,
- const Eigen::Vector3f& origin,
- Eigen::Affine3f& transformation)
+getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
+ const Eigen::Vector3f& z_axis,
+ const Eigen::Vector3f& origin,
+ Eigen::Affine3f& transformation)
{
getTransformationFromTwoUnitVectors(y_direction, z_axis, transformation);
Eigen::Vector3f translation = transformation*origin;
transformation(0,3)=-translation[0]; transformation(1,3)=-translation[1]; transformation(2,3)=-translation[2];
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Scalar> void
-pcl::getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
+getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
{
roll = std::atan2 (t (2, 1), t (2, 2));
pitch = asin (-t (2, 0));
yaw = std::atan2 (t (1, 0), t (0, 0));
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Scalar> void
-pcl::getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
- Scalar &x, Scalar &y, Scalar &z,
- Scalar &roll, Scalar &pitch, Scalar &yaw)
+getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
+ Scalar &x, Scalar &y, Scalar &z,
+ Scalar &roll, Scalar &pitch, Scalar &yaw)
{
x = t (0, 3);
y = t (1, 3);
yaw = std::atan2 (t (1, 0), t (0, 0));
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Scalar> void
-pcl::getTransformation (Scalar x, Scalar y, Scalar z,
- Scalar roll, Scalar pitch, Scalar yaw,
- Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
+getTransformation (Scalar x, Scalar y, Scalar z,
+ Scalar roll, Scalar pitch, Scalar yaw,
+ Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
{
Scalar A = std::cos (yaw), B = sin (yaw), C = std::cos (pitch), D = sin (pitch),
E = std::cos (roll), F = sin (roll), DE = D*E, DF = D*F;
t (3, 0) = 0; t (3, 1) = 0; t (3, 2) = 0; t (3, 3) = 1;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Derived> void
-pcl::saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file)
+saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file)
{
std::uint32_t rows = static_cast<std::uint32_t> (matrix.rows ()), cols = static_cast<std::uint32_t> (matrix.cols ());
file.write (reinterpret_cast<char*> (&rows), sizeof (rows));
}
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Derived> void
-pcl::loadBinary (Eigen::MatrixBase<Derived> const & matrix_, std::istream& file)
+loadBinary (Eigen::MatrixBase<Derived> const & matrix_, std::istream& file)
{
Eigen::MatrixBase<Derived> &matrix = const_cast<Eigen::MatrixBase<Derived> &> (matrix_);
}
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Derived, typename OtherDerived>
typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
-pcl::umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
+umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
{
#if EIGEN_VERSION_AT_LEAST (3, 3, 0)
return Eigen::umeyama (src, dst, with_scaling);
#endif
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Scalar> bool
-pcl::transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
- Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
+transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
{
if (line_in.innerSize () != 6 || line_out.innerSize () != 6)
{
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Scalar> void
-pcl::transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
- Eigen::Matrix<Scalar, 4, 1> &plane_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
+transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
+ Eigen::Matrix<Scalar, 4, 1> &plane_out,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
{
Eigen::Hyperplane < Scalar, 3 > plane;
plane.coeffs () << plane_in;
#endif
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Scalar> void
-pcl::transformPlane (const pcl::ModelCoefficients::Ptr plane_in,
- pcl::ModelCoefficients::Ptr plane_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
+transformPlane (const pcl::ModelCoefficients::Ptr plane_in,
+ pcl::ModelCoefficients::Ptr plane_out,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
{
std::vector<Scalar> values (plane_in->values.begin (), plane_in->values.end ());
Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ());
std::copy_n(v_plane_in.data (), 4, plane_in->values.begin ());
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Scalar> bool
-pcl::checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
- const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
- const Scalar norm_limit,
- const Scalar dot_limit)
+checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
+ const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
+ const Scalar norm_limit,
+ const Scalar dot_limit)
{
if (line_x.innerSize () != 6 || line_y.innerSize () != 6)
{
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
template <typename Scalar> bool
-pcl::transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
- const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
- const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
- const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
- Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
+transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
+ const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
+ const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
+ const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
+ Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
{
if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6)
{
return (true);
}
-#endif //PCL_COMMON_EIGEN_IMPL_HPP_
+} // namespace pcl
+
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
- * All rights reserved.
+ * All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
*
*/
-#ifndef PCL_GAUSSIAN_KERNEL_IMPL
-#define PCL_GAUSSIAN_KERNEL_IMPL
+#pragma once
+#include <pcl/common/gaussian.h>
#include <pcl/exceptions.h>
+namespace pcl
+{
+
template <typename PointT> void
-
pcl::GaussianKernel::convolveRows(const pcl::PointCloud<PointT> &input,
- std::function <float (const PointT& p)> field_accessor,
- const Eigen::VectorXf& kernel,
- pcl::PointCloud<float> &output) const
+GaussianKernel::convolveRows(const pcl::PointCloud<PointT> &input,
+ std::function <float (const PointT& p)> field_accessor,
+ const Eigen::VectorXf& kernel,
+ pcl::PointCloud<float> &output) const
{
assert(kernel.size () % 2 == 1);
int kernel_width = kernel.size () -1;
}
template <typename PointT> void
-
pcl::GaussianKernel::convolveCols(const pcl::PointCloud<PointT> &input,
- std::function <float (const PointT& p)> field_accessor,
- const Eigen::VectorXf& kernel,
- pcl::PointCloud<float> &output) const
+GaussianKernel::convolveCols(const pcl::PointCloud<PointT> &input,
+ std::function <float (const PointT& p)> field_accessor,
+ const Eigen::VectorXf& kernel,
+ pcl::PointCloud<float> &output) const
{
assert(kernel.size () % 2 == 1);
int kernel_width = kernel.size () -1;
}
}
-#endif
+} // namespace pcl
+
*
*/
-#ifndef PCL_COMMON_GENERATE_HPP_
-#define PCL_COMMON_GENERATE_HPP_
+#pragma once
+#include <pcl/common/generate.h>
#include <pcl/console/print.h>
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace common
+{
+
template <typename PointT, typename GeneratorT>
-
pcl::common::CloudGenerator<PointT, GeneratorT>::CloudGenerator ()
+CloudGenerator<PointT, GeneratorT>::CloudGenerator ()
: x_generator_ ()
, y_generator_ ()
, z_generator_ ()
{}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename GeneratorT>
-
pcl::common::CloudGenerator<PointT, GeneratorT>::CloudGenerator (const GeneratorParameters& params)
+CloudGenerator<PointT, GeneratorT>::CloudGenerator (const GeneratorParameters& params)
{
setParameters (params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename GeneratorT>
-
pcl::common::CloudGenerator<PointT, GeneratorT>::CloudGenerator (const GeneratorParameters& x_params,
- const GeneratorParameters& y_params,
- const GeneratorParameters& z_params)
+CloudGenerator<PointT, GeneratorT>::CloudGenerator (const GeneratorParameters& x_params,
+ const GeneratorParameters& y_params,
+ const GeneratorParameters& z_params)
: x_generator_ (x_params)
, y_generator_ (y_params)
, z_generator_ (z_params)
{}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename GeneratorT> void
-
pcl::common::CloudGenerator<PointT, GeneratorT>::setParameters (const GeneratorParameters& params)
+CloudGenerator<PointT, GeneratorT>::setParameters (const GeneratorParameters& params)
{
GeneratorParameters y_params = params;
y_params.seed += 1;
z_params.seed += 1;
x_generator_.setParameters (params);
y_generator_.setParameters (y_params);
- z_generator_.setParameters (z_params);
+ z_generator_.setParameters (z_params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename GeneratorT> void
-
pcl::common::CloudGenerator<PointT, GeneratorT>::setParametersForX (const GeneratorParameters& x_params)
+CloudGenerator<PointT, GeneratorT>::setParametersForX (const GeneratorParameters& x_params)
{
x_generator_.setParameters (x_params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename GeneratorT> void
-
pcl::common::CloudGenerator<PointT, GeneratorT>::setParametersForY (const GeneratorParameters& y_params)
+CloudGenerator<PointT, GeneratorT>::setParametersForY (const GeneratorParameters& y_params)
{
y_generator_.setParameters (y_params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename GeneratorT> void
-
pcl::common::CloudGenerator<PointT, GeneratorT>::setParametersForZ (const GeneratorParameters& z_params)
+CloudGenerator<PointT, GeneratorT>::setParametersForZ (const GeneratorParameters& z_params)
{
z_generator_.setParameters (z_params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename GeneratorT> const typename
pcl::common::CloudGenerator<PointT, GeneratorT>::GeneratorParameters&
-
pcl::common::CloudGenerator<PointT, GeneratorT>::getParametersForX () const
+
+template <typename PointT, typename GeneratorT> const typename CloudGenerator<PointT, GeneratorT>::GeneratorParameters&
+CloudGenerator<PointT, GeneratorT>::getParametersForX () const
{
x_generator_.getParameters ();
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename GeneratorT> const typename
pcl::common::CloudGenerator<PointT, GeneratorT>::GeneratorParameters&
-
pcl::common::CloudGenerator<PointT, GeneratorT>::getParametersForY () const
+
+template <typename PointT, typename GeneratorT> const typename CloudGenerator<PointT, GeneratorT>::GeneratorParameters&
+CloudGenerator<PointT, GeneratorT>::getParametersForY () const
{
y_generator_.getParameters ();
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename GeneratorT> const typename
pcl::common::CloudGenerator<PointT, GeneratorT>::GeneratorParameters&
-
pcl::common::CloudGenerator<PointT, GeneratorT>::getParametersForZ () const
+
+template <typename PointT, typename GeneratorT> const typename CloudGenerator<PointT, GeneratorT>::GeneratorParameters&
+CloudGenerator<PointT, GeneratorT>::getParametersForZ () const
{
z_generator_.getParameters ();
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename GeneratorT> PointT
-
pcl::common::CloudGenerator<PointT, GeneratorT>::get ()
+CloudGenerator<PointT, GeneratorT>::get ()
{
PointT p;
p.x = x_generator_.run ();
return (p);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename GeneratorT> int
-
pcl::common::CloudGenerator<PointT, GeneratorT>::fill (pcl::PointCloud<PointT>& cloud)
+CloudGenerator<PointT, GeneratorT>::fill (pcl::PointCloud<PointT>& cloud)
{
return (fill (cloud.width, cloud.height, cloud));
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename GeneratorT> int
-
pcl::common::CloudGenerator<PointT, GeneratorT>::fill (int width, int height, pcl::PointCloud<PointT>& cloud)
+CloudGenerator<PointT, GeneratorT>::fill (int width, int height, pcl::PointCloud<PointT>& cloud)
{
if (width < 1)
{
PCL_ERROR ("[pcl::common::CloudGenerator] Cloud width must be >= 1!\n");
return (-1);
}
-
+
if (height < 1)
{
PCL_ERROR ("[pcl::common::CloudGenerator] Cloud height must be >= 1!\n");
return (-1);
}
-
+
if (!cloud.empty ())
{
PCL_WARN ("[pcl::common::CloudGenerator] Cloud data will be erased with new data!\n");
}
-
+
cloud.width = width;
cloud.height = height;
cloud.resize (cloud.width * cloud.height);
return (0);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT>
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::CloudGenerator ()
+CloudGenerator<pcl::PointXY, GeneratorT>::CloudGenerator ()
: x_generator_ ()
, y_generator_ ()
{}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT>
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::CloudGenerator (const GeneratorParameters& x_params,
- const GeneratorParameters& y_params)
+CloudGenerator<pcl::PointXY, GeneratorT>::CloudGenerator (const GeneratorParameters& x_params,
+ const GeneratorParameters& y_params)
: x_generator_ (x_params)
, y_generator_ (y_params)
{}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT>
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::CloudGenerator (const GeneratorParameters& params)
+CloudGenerator<pcl::PointXY, GeneratorT>::CloudGenerator (const GeneratorParameters& params)
{
setParameters (params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT> void
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::setParameters (const GeneratorParameters& params)
+CloudGenerator<pcl::PointXY, GeneratorT>::setParameters (const GeneratorParameters& params)
{
x_generator_.setParameters (params);
GeneratorParameters y_params = params;
y_generator_.setParameters (y_params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT> void
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::setParametersForX (const GeneratorParameters& x_params)
+CloudGenerator<pcl::PointXY, GeneratorT>::setParametersForX (const GeneratorParameters& x_params)
{
x_generator_.setParameters (x_params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT> void
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::setParametersForY (const GeneratorParameters& y_params)
+CloudGenerator<pcl::PointXY, GeneratorT>::setParametersForY (const GeneratorParameters& y_params)
{
y_generator_.setParameters (y_params);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename GeneratorT> const typename pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::GeneratorParameters&
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::getParametersForX () const
+
+template <typename GeneratorT> const typename CloudGenerator<pcl::PointXY, GeneratorT>::GeneratorParameters&
+CloudGenerator<pcl::PointXY, GeneratorT>::getParametersForX () const
{
x_generator_.getParameters ();
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename GeneratorT> const typename pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::GeneratorParameters&
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::getParametersForY () const
+
+template <typename GeneratorT> const typename CloudGenerator<pcl::PointXY, GeneratorT>::GeneratorParameters&
+CloudGenerator<pcl::PointXY, GeneratorT>::getParametersForY () const
{
y_generator_.getParameters ();
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT> pcl::PointXY
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::get ()
+CloudGenerator<pcl::PointXY, GeneratorT>::get ()
{
pcl::PointXY p;
p.x = x_generator_.run ();
return (p);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT> int
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::fill (pcl::PointCloud<pcl::PointXY>& cloud)
+CloudGenerator<pcl::PointXY, GeneratorT>::fill (pcl::PointCloud<pcl::PointXY>& cloud)
{
return (fill (cloud.width, cloud.height, cloud));
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename GeneratorT> int
-
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::fill (int width, int height, pcl::PointCloud<pcl::PointXY>& cloud)
+CloudGenerator<pcl::PointXY, GeneratorT>::fill (int width, int height, pcl::PointCloud<pcl::PointXY>& cloud)
{
if (width < 1)
{
PCL_ERROR ("[pcl::common::CloudGenerator] Cloud width must be >= 1\n!");
return (-1);
}
-
+
if (height < 1)
{
PCL_ERROR ("[pcl::common::CloudGenerator] Cloud height must be >= 1\n!");
return (-1);
}
-
+
if (!cloud.empty ())
PCL_WARN ("[pcl::common::CloudGenerator] Cloud data will be erased with new data\n!");
return (0);
}
-#endif
+} // namespace common
+} // namespace pcl
+
*
*/
-#ifndef PCL_COMMON_INTENSITY_FIELD_ACCESSOR_IMPL_HPP
-#define PCL_COMMON_INTENSITY_FIELD_ACCESSOR_IMPL_HPP
+#pragma once
+#include <pcl/common/intensity.h>
#include <pcl/point_types.h>
+
+
namespace pcl
{
namespace common
}
}
-#endif
*
*/
-#ifndef PCL_COMMON_INTERSECTIONS_IMPL_HPP_
-#define PCL_COMMON_INTERSECTIONS_IMPL_HPP_
+#pragma once
+#include <pcl/common/intersections.h>
#include <pcl/pcl_macros.h>
#include <pcl/console/print.h>
-//////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
bool
-pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a,
- const Eigen::VectorXf &line_b,
- Eigen::Vector4f &point, double sqr_eps)
+lineWithLineIntersection (const Eigen::VectorXf &line_a,
+ const Eigen::VectorXf &line_b,
+ Eigen::Vector4f &point, double sqr_eps)
{
Eigen::Vector4f p1, p2;
lineToLineSegment (line_a, line_b, p1, p2);
return (false);
}
+
bool
-pcl::lineWithLineIntersection (const pcl::ModelCoefficients &line_a,
- const pcl::ModelCoefficients &line_b,
- Eigen::Vector4f &point, double sqr_eps)
+lineWithLineIntersection (const pcl::ModelCoefficients &line_a,
+ const pcl::ModelCoefficients &line_b,
+ Eigen::Vector4f &point, double sqr_eps)
{
Eigen::VectorXf coeff1 = Eigen::VectorXf::Map (&line_a.values[0], line_a.values.size ());
Eigen::VectorXf coeff2 = Eigen::VectorXf::Map (&line_b.values[0], line_b.values.size ());
return (lineWithLineIntersection (coeff1, coeff2, point, sqr_eps));
}
-template <typename Scalar> bool
-pcl::planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
- const Eigen::Matrix<Scalar, 4, 1> &plane_b,
- Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
- double angular_tolerance)
+template <typename Scalar> bool
+planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
+ const Eigen::Matrix<Scalar, 4, 1> &plane_b,
+ Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
+ double angular_tolerance)
{
using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
// Construct system of equations using lagrange multipliers with one objective function and two constraints
Matrix5 langrange_coefs;
- langrange_coefs << 2,0,0, plane_a[0], plane_b[0],
+ langrange_coefs << 2,0,0, plane_a[0], plane_b[0],
0,2,0, plane_a[1], plane_b[1],
0,0,2, plane_a[2], plane_b[2],
plane_a[0], plane_a[1], plane_a[2], 0, 0,
}
template <typename Scalar> bool
-pcl::threePlanesIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
- const Eigen::Matrix<Scalar, 4, 1> &plane_b,
- const Eigen::Matrix<Scalar, 4, 1> &plane_c,
- Eigen::Matrix<Scalar, 3, 1> &intersection_point,
- double determinant_tolerance)
+threePlanesIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
+ const Eigen::Matrix<Scalar, 4, 1> &plane_b,
+ const Eigen::Matrix<Scalar, 4, 1> &plane_c,
+ Eigen::Matrix<Scalar, 3, 1> &intersection_point,
+ double determinant_tolerance)
{
using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
return (true);
}
-#endif //PCL_COMMON_INTERSECTIONS_IMPL_HPP
+} // namespace pcl
+
*
*/
-#ifndef PCL_IO_IMPL_IO_HPP_
-#define PCL_IO_IMPL_IO_HPP_
+#pragma once
#include <pcl/common/concatenate.h>
#include <pcl/common/copy_point.h>
+#include <pcl/common/io.h>
#include <pcl/point_types.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointT> int
-pcl::getFieldIndex (const pcl::PointCloud<PointT> &,
- const std::string &field_name,
-
std::vector<pcl::PCLPointField> &fields)
+getFieldIndex (const pcl::PointCloud<PointT> &,
+ const std::string &field_name,
+ std::vector<pcl::PCLPointField> &fields)
{
return getFieldIndex<PointT>(field_name, fields);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> int
-pcl::getFieldIndex (const std::string &field_name,
-
std::vector<pcl::PCLPointField> &fields)
+getFieldIndex (const std::string &field_name,
+ std::vector<pcl::PCLPointField> &fields)
{
fields = getFields<PointT> ();
const auto& ref = fields;
return pcl::getFieldIndex<PointT> (field_name, ref);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> int
-pcl::getFieldIndex (const std::string &field_name,
-
const std::vector<pcl::PCLPointField> &fields)
+getFieldIndex (const std::string &field_name,
+ const std::vector<pcl::PCLPointField> &fields)
{
const auto result = std::find_if(fields.begin (), fields.end (),
[&field_name](const auto& field) { return field.name == field_name; });
return std::distance(fields.begin (), result);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-
pcl::getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
+getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
{
fields = getFields<PointT> ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-
pcl::getFields (std::vector<pcl::PCLPointField> &fields)
+getFields (std::vector<pcl::PCLPointField> &fields)
{
fields = getFields<PointT> ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> std::vector<pcl::PCLPointField>
-pcl::getFields ()
+getFields ()
{
std::vector<pcl::PCLPointField> fields;
// Get the fields list
return fields;
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> std::string
-
pcl::getFieldsList (const pcl::PointCloud<PointT> &)
+getFieldsList (const pcl::PointCloud<PointT> &)
{
// Get the fields list
const auto fields = getFields<PointT>();
return (result);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT> void
-
pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
-
pcl::PointCloud<PointOutT> &cloud_out)
+copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
+ pcl::PointCloud<PointOutT> &cloud_out)
{
// Allocate enough space and copy the basics
cloud_out.header = cloud_in.header;
copyPoint (cloud_in.points[i], cloud_out.points[i]);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename IndicesVectorAllocator> void
-pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const std::vector<int, IndicesVectorAllocator> &indices,
-
pcl::PointCloud<PointT> &cloud_out)
+copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<int, IndicesVectorAllocator> &indices,
+ pcl::PointCloud<PointT> &cloud_out)
{
// Do we want to copy everything?
if (indices.size () == cloud_in.points.size ())
cloud_out.points[i] = cloud_in.points[indices[i]];
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator> void
-
pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
- const std::vector<int, IndicesVectorAllocator> &indices,
-
pcl::PointCloud<PointOutT> &cloud_out)
+copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
+ const std::vector<int, IndicesVectorAllocator> &indices,
+ pcl::PointCloud<PointOutT> &cloud_out)
{
// Allocate enough space and copy the basics
cloud_out.points.resize (indices.size ());
copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const pcl::PointIndices &indices,
+copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out)
{
// Do we want to copy everything?
cloud_out.points[i] = cloud_in.points[indices.indices[i]];
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT> void
-
pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
- const pcl::PointIndices &indices,
-
pcl::PointCloud<PointOutT> &cloud_out)
+copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
+ const pcl::PointIndices &indices,
+ pcl::PointCloud<PointOutT> &cloud_out)
{
copyPointCloud (cloud_in, indices.indices, cloud_out);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
-
const std::vector<pcl::PointIndices> &indices,
-
pcl::PointCloud<PointT> &cloud_out)
+copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<pcl::PointIndices> &indices,
+ pcl::PointCloud<PointT> &cloud_out)
{
int nr_p = 0;
for (const auto &index : indices)
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT> void
-pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
-
const std::vector<pcl::PointIndices> &indices,
-
pcl::PointCloud<PointOutT> &cloud_out)
+copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
+ const std::vector<pcl::PointIndices> &indices,
+ pcl::PointCloud<PointOutT> &cloud_out)
{
const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
[](const auto& acc, const auto& index) { return index.indices.size() + acc; });
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
-
pcl::concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in,
-
const pcl::PointCloud<PointIn2T> &cloud2_in,
-
pcl::PointCloud<PointOutT> &cloud_out)
+concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in,
+ const pcl::PointCloud<PointIn2T> &cloud2_in,
+ pcl::PointCloud<PointOutT> &cloud_out)
{
using FieldList1 = typename pcl::traits::fieldList<PointIn1T>::type;
using FieldList2 = typename pcl::traits::fieldList<PointIn2T>::type;
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-
pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out,
- int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
+copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out,
+ int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
{
if (top < 0 || left < 0 || bottom < 0 || right < 0)
{
cloud_out.width * sizeof (PointT));
}
}
- catch (pcl::BadArgumentException &e)
+ catch (pcl::BadArgumentException&)
{
PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
}
}
}
-#endif // PCL_IO_IMPL_IO_H_
+} // namespace pcl
*
*/
-#ifndef PCL_COMMON_NORMS_IMPL_HPP_
-#define PCL_COMMON_NORMS_IMPL_HPP_
+#pragma once
-#include <pcl/pcl_macros.h>
+#include <pcl/common/norms.h>
#include <pcl/console/print.h>
+#include <pcl/pcl_macros.h>
+
namespace pcl
{
return norm;
}
-}
-#endif
+} // namespace pcl
* $Id$
*/
-#ifndef PCL_PCA_IMPL_HPP
-#define PCL_PCA_IMPL_HPP
+#pragma once
-#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/common/eigen.h>
+#include <pcl/common/pca.h>
#include <pcl/common/transforms.h>
+#include <pcl/point_types.h>
#include <pcl/exceptions.h>
-/////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template<typename PointT> bool
-pcl::PCA<PointT>::initCompute ()
+PCA<PointT>::initCompute ()
{
if(!Base::initCompute ())
{
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
}
-
+
// Compute mean
mean_ = Eigen::Vector4f::Zero ();
- compute3DCentroid (*input_, *indices_, mean_);
+ compute3DCentroid (*input_, *indices_, mean_);
// Compute demeanished cloud
Eigen::MatrixXf cloud_demean;
demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
// Compute the product cloud_demean * cloud_demean^T
const Eigen::Matrix3f alpha = (1.f / (float (indices_->size ()) - 1.f))
* cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();
-
+
// Compute eigen vectors and values
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
// Organize eigenvectors and eigenvalues in ascendent order
eigenvalues_[i] = evd.eigenvalues () [2-i];
eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
}
- // Enforce right hand rule
+ // Enforce right hand rule
eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1));
// If not basis only then compute the coefficients
if (!basis_only_)
return (true);
}
-/////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT> inline void
-pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
+
+template<typename PointT> inline void
+PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
- if (h.norm() > 0)
+ if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
- switch (flag)
+ switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
}
}
-/////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline void
-
pcl::PCA<PointT>::project (const PointT& input, PointT& projection)
+PCA<PointT>::project (const PointT& input, PointT& projection)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
-
+
Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
}
-/////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline void
-
pcl::PCA<PointT>::project (const PointCloud& input, PointCloud& projection)
+PCA<PointT>::project (const PointCloud& input, PointCloud& projection)
{
if(!compute_done_)
initCompute ();
}
}
-/////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline void
-
pcl::PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
+PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
{
if(!compute_done_)
initCompute ();
input.getVector3fMap ()+= mean_.head<3> ();
}
-/////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline void
-
pcl::PCA<PointT>::reconstruct (const PointCloud& projection, PointCloud& input)
+PCA<PointT>::reconstruct (const PointCloud& projection, PointCloud& input)
{
if(!compute_done_)
initCompute ();
PointT p;
for (std::size_t i = 0; i < input.size (); ++i)
{
- if (!std::isfinite (input[i].x) ||
+ if (!std::isfinite (input[i].x) ||
!std::isfinite (input[i].y) ||
!std::isfinite (input[i].z))
continue;
}
}
-#endif
+} // namespace pcl
+
/* \author Bastian Steder */
+#pragma once
+
+#include <pcl/common/piecewise_linear_function.h>
+
#include <algorithm>
#include <cmath>
// #include <iostream>
return data_points_[data_point_before]+interpolation_size*(data_points_[data_point_before+1]-data_points_[data_point_before]);
}
-} // end namespace pcl
+} // namespace pcl
* POSSIBILITY OF SUCH DAMAGE.
*
*/
-#ifndef PCL_POLYNOMIAL_CALCULATIONS_HPP_
-#define PCL_POLYNOMIAL_CALCULATIONS_HPP_
-////////////////////////////////////
+#pragma once
+
+#include <pcl/common/polynomial_calculations.h>
+
+
+namespace pcl
+{
template <typename real>
inline void
- pcl::PolynomialCalculationsT<real>::Parameters::setZeroValue (real new_zero_value)
+PolynomialCalculationsT<real>::Parameters::setZeroValue (real new_zero_value)
{
zero_value = new_zero_value;
sqr_zero_value = zero_value*zero_value;
}
-////////////////////////////////////
template <typename real>
inline void
- pcl::PolynomialCalculationsT<real>::solveLinearEquation (real a, real b, std::vector<real>& roots) const
+PolynomialCalculationsT<real>::solveLinearEquation (real a, real b, std::vector<real>& roots) const
{
//std::cout << "Trying to solve "<<a<<"x + "<<b<<" = 0\n";
#endif
}
-////////////////////////////////////
template <typename real>
inline void
- pcl::PolynomialCalculationsT<real>::solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const
+PolynomialCalculationsT<real>::solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const
{
//std::cout << "Trying to solve "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
#endif
}
-////////////////////////////////////
template<typename real>
inline void
- pcl::PolynomialCalculationsT<real>::solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const
+PolynomialCalculationsT<real>::solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const
{
//std::cout << "Trying to solve "<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = 0\n";
#endif
}
-////////////////////////////////////
template<typename real>
inline void
- pcl::PolynomialCalculationsT<real>::solveQuarticEquation (real a, real b, real c, real d, real e,
- std::vector<real>& roots) const
+PolynomialCalculationsT<real>::solveQuarticEquation (real a, real b, real c, real d, real e,
+ std::vector<real>& roots) const
{
//std::cout << "Trying to solve "<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = 0\n";
#endif
}
-////////////////////////////////////
template<typename real>
inline pcl::BivariatePolynomialT<real>
- pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
- std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree, bool& error) const
+PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
+ std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree, bool& error) const
{
pcl::BivariatePolynomialT<real> ret;
error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret);
return ret;
}
-////////////////////////////////////
template<typename real>
inline bool
- pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
- std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree,
- pcl::BivariatePolynomialT<real>& ret) const
+PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
+ std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree,
+ pcl::BivariatePolynomialT<real>& ret) const
{
const auto parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
return true;
}
-#endif // PCL_POLYNOMIAL_CALCULATIONS_HPP_
+} // namespace pcl
*
*/
-#ifndef __PCL_ORGANIZED_PROJECTION_MATRIX_HPP__
-#define __PCL_ORGANIZED_PROJECTION_MATRIX_HPP__
+#pragma once
+#include <pcl/common/projection_matrix.h>
#include <pcl/cloud_iterator.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
namespace pcl
{
- namespace common
+
+namespace common
+{
+
+namespace internal
+{
+
+template <typename MatrixT> void
+makeSymmetric (MatrixT& matrix, bool use_upper_triangular = true)
+{
+ if (use_upper_triangular && (MatrixT::Flags & Eigen::RowMajorBit))
{
- namespace internal
- {
- template <typename MatrixT> void
- makeSymmetric (MatrixT& matrix, bool use_upper_triangular = true)
- {
- if (use_upper_triangular && (MatrixT::Flags & Eigen::RowMajorBit))
- {
- matrix.coeffRef (4) = matrix.coeff (1);
- matrix.coeffRef (8) = matrix.coeff (2);
- matrix.coeffRef (9) = matrix.coeff (6);
- matrix.coeffRef (12) = matrix.coeff (3);
- matrix.coeffRef (13) = matrix.coeff (7);
- matrix.coeffRef (14) = matrix.coeff (11);
- }
- else
- {
- matrix.coeffRef (1) = matrix.coeff (4);
- matrix.coeffRef (2) = matrix.coeff (8);
- matrix.coeffRef (6) = matrix.coeff (9);
- matrix.coeffRef (3) = matrix.coeff (12);
- matrix.coeffRef (7) = matrix.coeff (13);
- matrix.coeffRef (11) = matrix.coeff (14);
- }
- }
- }
+ matrix.coeffRef (4) = matrix.coeff (1);
+ matrix.coeffRef (8) = matrix.coeff (2);
+ matrix.coeffRef (9) = matrix.coeff (6);
+ matrix.coeffRef (12) = matrix.coeff (3);
+ matrix.coeffRef (13) = matrix.coeff (7);
+ matrix.coeffRef (14) = matrix.coeff (11);
+ }
+ else
+ {
+ matrix.coeffRef (1) = matrix.coeff (4);
+ matrix.coeffRef (2) = matrix.coeff (8);
+ matrix.coeffRef (6) = matrix.coeff (9);
+ matrix.coeffRef (3) = matrix.coeff (12);
+ matrix.coeffRef (7) = matrix.coeff (13);
+ matrix.coeffRef (11) = matrix.coeff (14);
}
}
-//////////////////////////////////////////////////////////////////////////////
-template <typename PointT> double
-pcl::estimateProjectionMatrix (
- typename pcl::PointCloud<PointT>::ConstPtr cloud,
- Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix,
+} // namespace internal
+} // namespace common
+
+
+template <typename PointT> double
+estimateProjectionMatrix (
+ typename pcl::PointCloud<PointT>::ConstPtr cloud,
+ Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix,
const std::vector<int>& indices)
{
// internally we calculate with double but store the result into float matrices.
PCL_ERROR ("[pcl::estimateProjectionMatrix] Input dataset is not organized!\n");
return (-1.0);
}
-
+
Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
pcl::ConstCloudIterator <PointT> pointIt (*cloud, indices);
-
+
while (pointIt)
{
unsigned yIdx = pointIt.getCurrentPointIndex () / cloud->width;
unsigned xIdx = pointIt.getCurrentPointIndex () % cloud->width;
-
+
const PointT& point = *pointIt;
if (std::isfinite (point.x))
{
D.coeffRef (15) += xx_yy;
}
-
+
++pointIt;
- } // while
-
- pcl::common::internal::makeSymmetric (A);
- pcl::common::internal::makeSymmetric (B);
- pcl::common::internal::makeSymmetric (C);
- pcl::common::internal::makeSymmetric (D);
+ } // while
+
+ common::internal::makeSymmetric (A);
+ common::internal::makeSymmetric (B);
+ common::internal::makeSymmetric (C);
+ common::internal::makeSymmetric (D);
Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero ();
X.topLeftCorner<4,4> ().matrix () = A;
// check whether the residual MSE is low. If its high, the cloud was not captured from a projective device.
Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X * eigen_vectors.col (0);
-
+
double residual = residual_sqr.coeff (0);
projection_matrix.coeffRef (0) = static_cast <float> (eigen_vectors.coeff (0));
return (residual);
}
-#endif
+} // namespace pcl
+
*
*/
-#ifndef PCL_COMMON_RANDOM_HPP_
-#define PCL_COMMON_RANDOM_HPP_
+#pragma once
+
+#include <pcl/common/random.h>
+
+
+namespace pcl
+{
+
+namespace common
+{
+
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T>
-pcl::common::UniformGenerator<T>::UniformGenerator(T min, T max, std::uint32_t seed)
+UniformGenerator<T>::UniformGenerator(T min, T max, std::uint32_t seed)
: distribution_ (min, max)
{
parameters_ = Parameters (min, max, seed);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T>
-pcl::common::UniformGenerator<T>::UniformGenerator(const Parameters& parameters)
+UniformGenerator<T>::UniformGenerator(const Parameters& parameters)
: parameters_ (parameters)
, distribution_ (parameters_.min, parameters_.max)
{
rng_.seed (parameters_.seed);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename T> void
-pcl::common::UniformGenerator<T>::setSeed (std::uint32_t seed)
+UniformGenerator<T>::setSeed (std::uint32_t seed)
{
if (seed != static_cast<std::uint32_t> (-1))
{
}
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename T> void
-pcl::common::UniformGenerator<T>::setParameters (T min, T max, std::uint32_t seed)
+UniformGenerator<T>::setParameters (T min, T max, std::uint32_t seed)
{
parameters_.min = min;
parameters_.max = max;
}
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename T> void
-pcl::common::UniformGenerator<T>::setParameters (const Parameters& parameters)
+UniformGenerator<T>::setParameters (const Parameters& parameters)
{
parameters_ = parameters;
typename DistributionType::param_type params (parameters_.min, parameters_.max);
rng_.seed (parameters_.seed);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename T>
-pcl::common::NormalGenerator<T>::NormalGenerator(T mean, T sigma, std::uint32_t seed)
+NormalGenerator<T>::NormalGenerator(T mean, T sigma, std::uint32_t seed)
: distribution_ (mean, sigma)
{
parameters_ = Parameters (mean, sigma, seed);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T>
-pcl::common::NormalGenerator<T>::NormalGenerator(const Parameters& parameters)
+NormalGenerator<T>::NormalGenerator(const Parameters& parameters)
: parameters_ (parameters)
, distribution_ (parameters_.mean, parameters_.sigma)
{
rng_.seed (parameters_.seed);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename T> void
-pcl::common::NormalGenerator<T>::setSeed (std::uint32_t seed)
+NormalGenerator<T>::setSeed (std::uint32_t seed)
{
if (seed != static_cast<std::uint32_t> (-1))
{
}
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename T> void
-pcl::common::NormalGenerator<T>::setParameters (T mean, T sigma, std::uint32_t seed)
+NormalGenerator<T>::setParameters (T mean, T sigma, std::uint32_t seed)
{
parameters_.mean = mean;
parameters_.sigma = sigma;
rng_.seed (parameters_.seed);
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename T> void
-pcl::common::NormalGenerator<T>::setParameters (const Parameters& parameters)
+NormalGenerator<T>::setParameters (const Parameters& parameters)
{
parameters_ = parameters;
typename DistributionType::param_type params (parameters_.mean, parameters_.sigma);
rng_.seed (parameters_.seed);
}
-#endif
+} // namespace common
+} // namespace pcl
+
*
*/
-#ifndef PCL_POINT_CLOUD_SPRING_IMPL_HPP_
-#define PCL_POINT_CLOUD_SPRING_IMPL_HPP_
+#pragma once
-template <typename PointT> void
-pcl::common::expandColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
- const PointT& val, const std::size_t& amount)
+#include <pcl/common/spring.h>
+
+
+namespace pcl
+{
+
+namespace common
+{
+
+template <typename PointT> void
+expandColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
+ const PointT& val, const std::size_t& amount)
{
if (amount <= 0)
PCL_THROW_EXCEPTION (InitFailedException,
if (!input.isOrganized () || amount > (input.width/2))
PCL_THROW_EXCEPTION (InitFailedException,
- "[pcl::common::expandColumns] error: "
+ "[pcl::common::expandColumns] error: "
<< "columns expansion requires organised point cloud");
std::uint32_t old_height = input.height;
output.height = old_height;
}
-template <typename PointT> void
-
pcl::common::expandRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
- const PointT& val, const std::size_t& amount)
+template <typename PointT> void
+expandRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
+ const PointT& val, const std::size_t& amount)
{
if (amount <= 0)
PCL_THROW_EXCEPTION (InitFailedException,
output.height = new_height;
}
-template <typename PointT> void
-
pcl::common::duplicateColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
- const std::size_t& amount)
+template <typename PointT> void
+duplicateColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
+ const std::size_t& amount)
{
if (amount <= 0)
PCL_THROW_EXCEPTION (InitFailedException,
if (!input.isOrganized () || amount > (input.width/2))
PCL_THROW_EXCEPTION (InitFailedException,
- "[pcl::common::duplicateColumns] error: "
+ "[pcl::common::duplicateColumns] error: "
<< "columns expansion requires organised point cloud");
std::size_t old_height = input.height;
output.height = old_height;
}
-template <typename PointT> void
-
pcl::common::duplicateRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
- const std::size_t& amount)
+template <typename PointT> void
+duplicateRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
+ const std::size_t& amount)
{
if (amount <= 0 || amount > (input.height/2))
PCL_THROW_EXCEPTION (InitFailedException,
- "[pcl::common::duplicateRows] error: amount must be ]0.."
+ "[pcl::common::duplicateRows] error: amount must be ]0.."
<< (input.height/2) << "] !");
std::uint32_t old_height = input.height;
output.height = new_height;
}
-template <typename PointT> void
-
pcl::common::mirrorColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
- const std::size_t& amount)
+template <typename PointT> void
+mirrorColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
+ const std::size_t& amount)
{
if (amount <= 0)
PCL_THROW_EXCEPTION (InitFailedException,
if (!input.isOrganized () || amount > (input.width/2))
PCL_THROW_EXCEPTION (InitFailedException,
- "[pcl::common::mirrorColumns] error: "
+ "[pcl::common::mirrorColumns] error: "
<< "columns expansion requires organised point cloud");
std::size_t old_height = input.height;
output.height = old_height;
}
-template <typename PointT> void
-
pcl::common::mirrorRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
- const std::size_t& amount)
+template <typename PointT> void
+mirrorRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
+ const std::size_t& amount)
{
if (amount <= 0 || amount > (input.height/2))
PCL_THROW_EXCEPTION (InitFailedException,
- "[pcl::common::mirrorRows] error: amount must be ]0.."
+ "[pcl::common::mirrorRows] error: amount must be ]0.."
<< (input.height/2) << "] !");
std::uint32_t old_height = input.height;
output.height = new_height;
}
-template <typename PointT> void
-
pcl::common::deleteRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
- const std::size_t& amount)
+template <typename PointT> void
+deleteRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
+ const std::size_t& amount)
{
if (amount <= 0 || amount > (input.height/2))
PCL_THROW_EXCEPTION (InitFailedException,
- "[pcl::common::deleteRows] error: amount must be ]0.."
+ "[pcl::common::deleteRows] error: amount must be ]0.."
<< (input.height/2) << "] !");
std::uint32_t old_height = input.height;
output.width = old_width;
}
-template <typename PointT> void
-
pcl::common::deleteCols (const PointCloud<PointT>& input, PointCloud<PointT>& output,
- const std::size_t& amount)
+template <typename PointT> void
+deleteCols (const PointCloud<PointT>& input, PointCloud<PointT>& output,
+ const std::size_t& amount)
{
if (amount <= 0 || amount > (input.width/2))
PCL_THROW_EXCEPTION (InitFailedException,
if (!input.isOrganized ())
PCL_THROW_EXCEPTION (InitFailedException,
- "[pcl::common::deleteCols] error: "
+ "[pcl::common::deleteCols] error: "
<< "columns delete requires organised point cloud");
std::uint32_t old_height = input.height;
typename PointCloud<PointT>::iterator start = output.begin () + j * new_width;
output.erase (start, start + amount);
start = output.begin () + (j+1) * new_width;
- output.erase (start, start + amount);
+ output.erase (start, start + amount);
}
output.height = old_height;
output.width = new_width;
}
-#endif
+} // namespace common
+} // namespace pcl
+
*
*/
+#pragma once
+
#include <pcl/common/eigen.h>
+#include <pcl/common/transformation_from_correspondences.h>
+
+
+namespace pcl
+{
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline void
-pcl::TransformationFromCorrespondences::reset ()
+TransformationFromCorrespondences::reset ()
{
no_of_samples_ = 0;
accumulated_weight_ = 0.0;
covariance_.fill(0);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
inline void
-pcl::TransformationFromCorrespondences::add (const Eigen::Vector3f& point, const Eigen::Vector3f& corresponding_point,
- float weight)
+TransformationFromCorrespondences::add (const Eigen::Vector3f& point, const Eigen::Vector3f& corresponding_point,
+ float weight)
{
if (weight==0.0f)
return;
-
+
++no_of_samples_;
accumulated_weight_ += weight;
float alpha = weight/accumulated_weight_;
-
+
Eigen::Vector3f diff1 = point - mean1_, diff2 = corresponding_point - mean2_;
covariance_ = (1.0f-alpha)*(covariance_ + alpha * (diff2 * diff1.transpose()));
-
+
mean1_ += alpha*(diff1);
mean2_ += alpha*(diff2);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
inline Eigen::Affine3f
-pcl::TransformationFromCorrespondences::getTransformation ()
+TransformationFromCorrespondences::getTransformation ()
{
//Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
s.setIdentity();
if (u.determinant()*v.determinant() < 0.0f)
s(2,2) = -1.0f;
-
+
Eigen::Matrix<float, 3, 3> r = u * s * v.transpose();
Eigen::Vector3f t = mean2_ - r*mean1_;
-
+
Eigen::Affine3f ret;
ret(0,0)=r(0,0); ret(0,1)=r(0,1); ret(0,2)=r(0,2); ret(0,3)=t(0);
ret(1,0)=r(1,0); ret(1,1)=r(1,1); ret(1,2)=r(1,2); ret(1,3)=t(1);
ret(2,0)=r(2,0); ret(2,1)=r(2,1); ret(2,2)=r(2,2); ret(2,3)=t(2);
ret(3,0)=0.0f; ret(3,1)=0.0f; ret(3,2)=0.0f; ret(3,3)=1.0f;
-
+
return (ret);
}
+
+} // namespace pcl
+
*
*/
+#pragma once
+
+#include <pcl/common/transforms.h>
+
#if defined(__SSE2__)
#include <xmmintrin.h>
#endif
#include <cstddef>
#include <vector>
+
namespace pcl
{
- namespace detail
+namespace detail
+{
+
+/** A helper struct to apply an SO3 or SE3 transform to a 3D point.
+ * Supports single and double precision transform matrices. */
+template<typename Scalar>
+struct Transformer
+{
+ const Eigen::Matrix<Scalar, 4, 4>& tf;
+
+ /** Construct a transformer object.
+ * The transform matrix is captured by const reference. Make sure that it does not go out of scope before this
+ * object does. */
+ Transformer (const Eigen::Matrix<Scalar, 4, 4>& transform) : tf (transform) { };
+
+ /** Apply SO3 transform (top-left corner of the transform matrix).
+ * \param[in] src input 3D point (pointer to 3 floats)
+ * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 0. */
+ void so3 (const float* src, float* tgt) const
{
+ const Scalar p[3] = { src[0], src[1], src[2] }; // need this when src == tgt
+ tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2]);
+ tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2]);
+ tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2]);
+ tgt[3] = 0;
+ }
- /** A helper struct to apply an SO3 or SE3 transform to a 3D point.
- * Supports single and double precision transform matrices. */
- template<typename Scalar>
- struct Transformer
- {
- const Eigen::Matrix<Scalar, 4, 4>& tf;
-
- /** Construct a transformer object.
- * The transform matrix is captured by const reference. Make sure that it does not go out of scope before this
- * object does. */
- Transformer (const Eigen::Matrix<Scalar, 4, 4>& transform) : tf (transform) { };
-
- /** Apply SO3 transform (top-left corner of the transform matrix).
- * \param[in] src input 3D point (pointer to 3 floats)
- * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 0. */
- void so3 (const float* src, float* tgt) const
- {
- const Scalar p[3] = { src[0], src[1], src[2] }; // need this when src == tgt
- tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2]);
- tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2]);
- tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2]);
- tgt[3] = 0;
- }
-
- /** Apply SE3 transform.
- * \param[in] src input 3D point (pointer to 3 floats)
- * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 1. */
- void se3 (const float* src, float* tgt) const
- {
- const Scalar p[3] = { src[0], src[1], src[2] }; // need this when src == tgt
- tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2] + tf (0, 3));
- tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2] + tf (1, 3));
- tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2] + tf (2, 3));
- tgt[3] = 1;
- }
- };
+ /** Apply SE3 transform.
+ * \param[in] src input 3D point (pointer to 3 floats)
+ * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 1. */
+ void se3 (const float* src, float* tgt) const
+ {
+ const Scalar p[3] = { src[0], src[1], src[2] }; // need this when src == tgt
+ tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2] + tf (0, 3));
+ tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2] + tf (1, 3));
+ tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2] + tf (2, 3));
+ tgt[3] = 1;
+ }
+};
#if defined(__SSE2__)
- /** Optimized version for single-precision transforms using SSE2 intrinsics. */
- template<>
- struct Transformer<float>
- {
- /// Columns of the transform matrix stored in XMM registers.
- __m128 c[4];
-
- Transformer(const Eigen::Matrix4f& tf)
- {
- for (std::size_t i = 0; i < 4; ++i)
- c[i] = _mm_load_ps (tf.col (i).data ());
- }
-
- void so3 (const float* src, float* tgt) const
- {
- __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
- __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
- __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
- _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, p2)));
- }
-
- void se3 (const float* src, float* tgt) const
- {
- __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
- __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
- __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
- _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, _mm_add_ps(p2, c[3]))));
- }
- };
-
-#if !defined(__AVX__)
+/** Optimized version for single-precision transforms using SSE2 intrinsics. */
+template<>
+struct Transformer<float>
+{
+ /// Columns of the transform matrix stored in XMM registers.
+ __m128 c[4];
- /** Optimized version for double-precision transform using SSE2 intrinsics. */
- template<>
- struct Transformer<double>
- {
- /// Columns of the transform matrix stored in XMM registers.
- __m128d c[4][2];
-
- Transformer(const Eigen::Matrix4d& tf)
- {
- for (std::size_t i = 0; i < 4; ++i)
- {
- c[i][0] = _mm_load_pd (tf.col (i).data () + 0);
- c[i][1] = _mm_load_pd (tf.col (i).data () + 2);
- }
- }
-
- void so3 (const float* src, float* tgt) const
- {
- __m128d xx = _mm_cvtps_pd (_mm_load_ps1 (&src[0]));
- __m128d p0 = _mm_mul_pd (xx, c[0][0]);
- __m128d p1 = _mm_mul_pd (xx, c[0][1]);
-
- for (std::size_t i = 1; i < 3; ++i)
- {
- __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
- p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
- p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
- }
-
- _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
- }
-
- void se3 (const float* src, float* tgt) const
- {
- __m128d p0 = c[3][0];
- __m128d p1 = c[3][1];
-
- for (std::size_t i = 0; i < 3; ++i)
- {
- __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
- p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
- p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
- }
-
- _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
- }
-
- };
+ Transformer(const Eigen::Matrix4f& tf)
+ {
+ for (std::size_t i = 0; i < 4; ++i)
+ c[i] = _mm_load_ps (tf.col (i).data ());
+ }
-#else
+ void so3 (const float* src, float* tgt) const
+ {
+ __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
+ __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
+ __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
+ _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, p2)));
+ }
- /** Optimized version for double-precision transform using AVX intrinsics. */
- template<>
- struct Transformer<double>
+ void se3 (const float* src, float* tgt) const
{
- __m256d c[4];
+ __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
+ __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
+ __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
+ _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, _mm_add_ps(p2, c[3]))));
+ }
+};
- Transformer(const Eigen::Matrix4d& tf)
+#if !defined(__AVX__)
+
+/** Optimized version for double-precision transform using SSE2 intrinsics. */
+template<>
+struct Transformer<double>
+{
+ /// Columns of the transform matrix stored in XMM registers.
+ __m128d c[4][2];
+
+ Transformer(const Eigen::Matrix4d& tf)
+ {
+ for (std::size_t i = 0; i < 4; ++i)
{
- for (std::size_t i = 0; i < 4; ++i)
- c[i] = _mm256_load_pd (tf.col (i).data ());
+ c[i][0] = _mm_load_pd (tf.col (i).data () + 0);
+ c[i][1] = _mm_load_pd (tf.col (i).data () + 2);
}
+ }
+
+ void so3 (const float* src, float* tgt) const
+ {
+ __m128d xx = _mm_cvtps_pd (_mm_load_ps1 (&src[0]));
+ __m128d p0 = _mm_mul_pd (xx, c[0][0]);
+ __m128d p1 = _mm_mul_pd (xx, c[0][1]);
- void so3 (const float* src, float* tgt) const
+ for (std::size_t i = 1; i < 3; ++i)
{
- __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
- __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
- __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
- _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, p2))));
+ __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
+ p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
+ p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
}
- void se3 (const float* src, float* tgt) const
+ _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
+ }
+
+ void se3 (const float* src, float* tgt) const
+ {
+ __m128d p0 = c[3][0];
+ __m128d p1 = c[3][1];
+
+ for (std::size_t i = 0; i < 3; ++i)
{
- __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
- __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
- __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
- _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, _mm256_add_pd(p2, c[3])))));
+ __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
+ p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
+ p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
}
- };
+ _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
+ }
+};
-#endif
-#endif
+#else
+
+/** Optimized version for double-precision transform using AVX intrinsics. */
+template<>
+struct Transformer<double>
+{
+ __m256d c[4];
+ Transformer(const Eigen::Matrix4d& tf)
+ {
+ for (std::size_t i = 0; i < 4; ++i)
+ c[i] = _mm256_load_pd (tf.col (i).data ());
}
-}
+ void so3 (const float* src, float* tgt) const
+ {
+ __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
+ __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
+ __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
+ _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, p2))));
+ }
+
+ void se3 (const float* src, float* tgt) const
+ {
+ __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
+ __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
+ __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
+ _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, _mm256_add_pd(p2, c[3])))));
+ }
+};
+
+#endif // !defined(__AVX__)
+#endif // defined(__SSE2__)
+
+} // namespace detail
+
-///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> void
-
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
-
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
- bool copy_all_fields)
+transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ pcl::PointCloud<PointT> &cloud_out,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields)
{
if (&cloud_in != &cloud_out)
{
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const std::vector<int> &indices,
-
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
- bool copy_all_fields)
+transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<int> &indices,
+ pcl::PointCloud<PointT> &cloud_out,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields)
{
std::size_t npts = indices.size ();
// In order to transform the data, we need to remove NaNs
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
-
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
- bool copy_all_fields)
+transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
+ pcl::PointCloud<PointT> &cloud_out,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields)
{
if (&cloud_in != &cloud_out)
{
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> void
-
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
- const std::vector<int> &indices,
-
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
- bool copy_all_fields)
+transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<int> &indices,
+ pcl::PointCloud<PointT> &cloud_out,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields)
{
std::size_t npts = indices.size ();
// In order to transform the data, we need to remove NaNs
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline void
-
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
-
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 3, 1> &offset,
- const Eigen::Quaternion<Scalar> &rotation,
- bool copy_all_fields)
+transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ pcl::PointCloud<PointT> &cloud_out,
+ const Eigen::Matrix<Scalar, 3, 1> &offset,
+ const Eigen::Quaternion<Scalar> &rotation,
+ bool copy_all_fields)
{
Eigen::Translation<Scalar, 3> translation (offset);
// Assemble an Eigen Transform
transformPointCloud (cloud_in, cloud_out, t, copy_all_fields);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline void
-
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
-
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 3, 1> &offset,
- const Eigen::Quaternion<Scalar> &rotation,
- bool copy_all_fields)
+transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
+ pcl::PointCloud<PointT> &cloud_out,
+ const Eigen::Matrix<Scalar, 3, 1> &offset,
+ const Eigen::Quaternion<Scalar> &rotation,
+ bool copy_all_fields)
{
Eigen::Translation<Scalar, 3> translation (offset);
// Assemble an Eigen Transform
transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline PointT
-pcl::transformPoint (const PointT &point,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+transformPoint (const PointT &point, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
pcl::detail::Transformer<Scalar> tf (transform.matrix ());
return (ret);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> inline PointT
-pcl::transformPointWithNormal (const PointT &point,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+transformPointWithNormal (const PointT &point, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
pcl::detail::Transformer<Scalar> tf (transform.matrix ());
return (ret);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT, typename Scalar> double
-
pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud,
- Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud,
+ Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
Eigen::Matrix<Scalar, 4, 1> centroid;
return (std::min (rel1, rel2));
}
+} // namespace pcl
+
* POSSIBILITY OF SUCH DAMAGE.
*/
-#ifndef PCL_COMMON_VECTOR_AVERAGE_IMPL_HPP_
-#define PCL_COMMON_VECTOR_AVERAGE_IMPL_HPP_
+#pragma once
+
+#include <pcl/common/vector_average.h>
+
namespace pcl
{
eigen33(covariance_, eigen_value, eigen_vector);
eigen_vector1 = eigen_vector;
}
-} // END namespace
-
-#endif
-
+} // namespace pcl
* \ingroup common
*/
template <typename PointT>
- PCL_DEPRECATED("use getFieldIndex<PointT> (field_name, fields) instead")
+ PCL_DEPRECATED(
1, 12, "use getFieldIndex<PointT> (field_name, fields) instead")
inline int
getFieldIndex (const pcl::PointCloud<PointT> &cloud, const std::string &field_name,
std::vector<pcl::PCLPointField> &fields);
* \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
* \ingroup common
*/
- template <typename PointT> inline int
- getFieldIndex (const std::string &field_name,
+ template <typename PointT> inline int
+ getFieldIndex (const std::string &field_name,
std::vector<pcl::PCLPointField> &fields);
/** \brief Get the index of a specified field (i.e., dimension/channel)
* \tparam PointT datatype for which fields is being queries
* \ingroup common
*/
template <typename PointT>
- PCL_DEPRECATED("use getFields<PointT> () with return value instead")
+ PCL_DEPRECATED(
1, 12, "use getFields<PointT> () with return value instead")
inline void
getFields (const pcl::PointCloud<PointT> &cloud, std::vector<pcl::PCLPointField> &fields);
* \ingroup common
*/
template <typename PointT>
- PCL_DEPRECATED("use getFields<PointT> () with return value instead")
- inline void
+ PCL_DEPRECATED(
1, 12, "use getFields<PointT> () with return value instead")
+ inline void
getFields (std::vector<pcl::PCLPointField> &fields);
/** \brief Get the list of available fields (i.e., dimension/channel)
* \param[in] cloud the point cloud message
* \ingroup common
*/
- template <typename PointT> inline std::string
+ template <typename PointT> inline std::string
getFieldsList (const pcl::PointCloud<PointT> &cloud);
/** \brief Get the available point cloud fields as a space separated string
/** \brief Concatenate two pcl::PCLPointCloud2
*
- * \warn This function subtly differs from the deprecated `concatenatePointloud`
- * The difference is thatthis function will concatenate IFF the non-skip fields
+ * \warning This function subtly differs from the deprecated concatenatePointCloud()
+ * The difference is that this function will concatenate IFF the non-skip fields
* are in the correct order and same in number. The deprecated function skipped
* fields even if both clouds didn't agree on the number of output fields
* \param[in] cloud1 the first input point cloud dataset
* \return true if successful, false otherwise (e.g., name/number of fields differs)
* \ingroup common
*/
- PCL_DEPRECATED("use pcl::concatenate() instead, but beware of subtle difference in behavior (see documentation)")
- PCL_EXPORTS bool
+ PCL_DEPRECATED(1, 12, "use pcl::concatenate() instead, but beware of subtle difference in behavior (see documentation)")
+ PCL_EXPORTS bool
concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1,
const pcl::PCLPointCloud2 &cloud2,
pcl::PCLPointCloud2 &cloud_out);
* \note Assumes unique indices.
* \ingroup common
*/
- PCL_EXPORTS void
+ PCL_EXPORTS void
copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
- const std::vector<int> &indices,
+ const std::vector<int> &indices,
pcl::PCLPointCloud2 &cloud_out);
/** \brief Extract the indices of a given point cloud as a new point cloud
* \note Assumes unique indices.
* \ingroup common
*/
- PCL_EXPORTS void
+ PCL_EXPORTS void
copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
- const std::vector<int, Eigen::aligned_allocator<int> > &indices,
+ const std::vector<int, Eigen::aligned_allocator<int> > &indices,
pcl::PCLPointCloud2 &cloud_out);
/** \brief Copy fields and point cloud data from \a cloud_in to \a cloud_out
* \param[out] cloud_out the resultant output point cloud dataset
* \ingroup common
*/
- PCL_EXPORTS void
+ PCL_EXPORTS void
copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
pcl::PCLPointCloud2 &cloud_out);
* \ingroup common
*/
template <typename PointT, typename IndicesVectorAllocator = std::allocator<int>> void
- copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int, IndicesVectorAllocator> &indices,
pcl::PointCloud<PointT> &cloud_out);
* \note Assumes unique indices.
* \ingroup common
*/
- template <typename PointT> void
- copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const PointIndices &indices,
+ template <typename PointT> void
+ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out);
/** \brief Extract the indices of a given point cloud as a new point cloud
* \note Assumes unique indices.
* \ingroup common
*/
- template <typename PointT> void
- copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const std::vector<pcl::PointIndices> &indices,
+ template <typename PointT> void
+ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<pcl::PointIndices> &indices,
pcl::PointCloud<PointT> &cloud_out);
/** \brief Copy all the fields from a given point cloud into a new point cloud
* \param[out] cloud_out the resultant output point cloud dataset
* \ingroup common
*/
- template <typename PointInT, typename PointOutT> void
- copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
+ template <typename PointInT, typename PointOutT> void
+ copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
pcl::PointCloud<PointOutT> &cloud_out);
/** \brief Extract the indices of a given point cloud as a new point cloud
* \ingroup common
*/
template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator = std::allocator<int>> void
- copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
+ copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
const std::vector<int, IndicesVectorAllocator> &indices,
pcl::PointCloud<PointOutT> &cloud_out);
* \note Assumes unique indices.
* \ingroup common
*/
- template <typename PointInT, typename PointOutT> void
- copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
- const PointIndices &indices,
+ template <typename PointInT, typename PointOutT> void
+ copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
+ const PointIndices &indices,
pcl::PointCloud<PointOutT> &cloud_out);
/** \brief Extract the indices of a given point cloud as a new point cloud
* \note Assumes unique indices.
* \ingroup common
*/
- template <typename PointInT, typename PointOutT> void
- copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
- const std::vector<pcl::PointIndices> &indices,
+ template <typename PointInT, typename PointOutT> void
+ copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
+ const std::vector<pcl::PointIndices> &indices,
pcl::PointCloud<PointOutT> &cloud_out);
/** \brief Copy a point cloud inside a larger one interpolating borders.
* \param[out] cloud_out the resultant output dataset created by the concatenation of all the fields in the input datasets
* \ingroup common
*/
- template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
- concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in,
- const pcl::PointCloud<PointIn2T> &cloud2_in,
+ template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
+ concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in,
+ const pcl::PointCloud<PointIn2T> &cloud2_in,
pcl::PointCloud<PointOutT> &cloud_out);
/** \brief Concatenate two datasets representing different fields.
* \param[out] out the resultant Eigen MatrixXf format containing XYZ0 / point
* \ingroup common
*/
- PCL_EXPORTS bool
+ PCL_EXPORTS bool
getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out);
/** \brief Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message
* \note the method assumes that the PCLPointCloud2 message already has the fields set up properly !
* \ingroup common
*/
- PCL_EXPORTS bool
+ PCL_EXPORTS bool
getEigenAsPointCloud (Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out);
-
- namespace io
+
+ namespace io
{
/** \brief swap bytes order of a char array of length N
* \param bytes char array to swap
* \ingroup common
*/
- template <std::size_t N> void
+ template <std::size_t N> void
swapByte (char* bytes);
/** \brief specialization of swapByte for dimension 1
* \param bytes char array to swap
*/
- template <> inline void
+ template <> inline void
swapByte<1> (char* bytes) { bytes[0] = bytes[0]; }
-
+
/** \brief specialization of swapByte for dimension 2
* \param bytes char array to swap
*/
- template <> inline void
+ template <> inline void
swapByte<2> (char* bytes) { std::swap (bytes[0], bytes[1]); }
-
+
/** \brief specialization of swapByte for dimension 4
* \param bytes char array to swap
*/
- template <> inline void
+ template <> inline void
swapByte<4> (char* bytes)
{
std::swap (bytes[0], bytes[3]);
std::swap (bytes[1], bytes[2]);
}
-
+
/** \brief specialization of swapByte for dimension 8
* \param bytes char array to swap
*/
- template <> inline void
+ template <> inline void
swapByte<8> (char* bytes)
{
std::swap (bytes[0], bytes[7]);
std::swap (bytes[2], bytes[5]);
std::swap (bytes[3], bytes[4]);
}
-
+
/** \brief swaps byte of an arbitrary type T casting it to char*
* \param value the data you want its bytes swapped
*/
- template <typename T> void
+ template <typename T> void
swapByte (T& value)
{
pcl::io::swapByte<sizeof(T)> (reinterpret_cast<char*> (&value));
+++ /dev/null
-/*
- * Software License Agreement (BSD License)
- *
- * Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2010-2011, Willow Garage, Inc.
- * Copyright (c) 2012-, Open Perception, Inc.
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the copyright holder(s) nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * $Id$
- *
- */
-
-#pragma once
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/correspondence.h>
/** Get the number of added vectors */
inline unsigned int
- getNoOfSamples () { return no_of_samples_;}
+ getNoOfSamples () const { return no_of_samples_;}
/** Add a new sample */
inline void
{
return (std::fabs (val1 - val2) < eps);
}
- }
-}
+
+ /** \brief Utility function to eliminate unused variable warnings. */
+ template<typename T> void
+ ignore(const T&)
+ {
+ }
+ } // namespace utils
+} // namespace pcl
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/common/eigen.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/PCLImage.h>
#include <pcl/point_cloud.h>
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
#include <pcl/for_each_type.h>
#include <pcl/exceptions.h>
#include <pcl/console/print.h>
#pragma once
#ifdef __GNUC__
-#pragma GCC system_header
+#pragma GCC system_header
#endif
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <pcl/pcl_exports.h>
namespace pcl
{
- /** \brief Correspondence represents a match between two entities (e.g., points, descriptors, etc). This is
+ /** \brief Correspondence represents a match between two entities (e.g., points, descriptors, etc). This is
* represented via the indices of a \a source point and a \a target point, and the distance between them.
*
* \author Dirk Holz, Radu B. Rusu, Bastian Steder
float distance = std::numeric_limits<float>::max();
float weight;
};
-
- /** \brief Standard constructor.
+
+ /** \brief Standard constructor.
* Sets \ref index_query to 0, \ref index_match to -1, and \ref distance to FLT_MAX.
*/
inline Correspondence () = default;
/** \brief Constructor. */
- inline Correspondence (int _index_query, int _index_match, float _distance) :
+ inline Correspondence (int _index_query, int _index_match, float _distance) :
index_query (_index_query), index_match (_index_match), distance (_distance)
{}
*
*/
-#ifndef PCL_IMPL_POINT_TYPES_HPP_
-#define PCL_IMPL_POINT_TYPES_HPP_
+#pragma once
-#include <cstdint>
-#if defined __GNUC__
-# pragma GCC system_header
-#endif
+#include <pcl/memory.h> // for PCL_MAKE_ALIGNED_OPERATOR_NEW
+#include <pcl/pcl_macros.h> // for PCL_EXPORTS
+#include <pcl/PCLPointField.h> // for PCLPointField
+#include <pcl/point_types.h> // implementee
+#include <pcl/register_point_struct.h> // for POINT_CLOUD_REGISTER_POINT_STRUCT, POINT_CLOUD_REGISTER_POINT_WRAPPER
-#include <algorithm>
-#include <ostream>
+#include <boost/mpl/and.hpp> // for boost::mpl::and_
+#include <boost/mpl/bool.hpp> // for boost::mpl::bool_
+#include <boost/mpl/contains.hpp> // for boost::mpl::contains
+#include <boost/mpl/fold.hpp> // for boost::mpl::fold
+#include <boost/mpl/or.hpp> // for boost::mpl::or_
+#include <boost/mpl/placeholders.hpp> // for boost::mpl::_1, boost::mpl::_2
+#include <boost/mpl/vector.hpp> // for boost::mpl::vector
-#include <Eigen/Core>
+#include <Eigen/Core> // for MatrixMap
-#include <pcl/pcl_macros.h>
+#include <algorithm> // for copy_n, fill_n
+#include <cstdint> // for uint8_t, uint32_t
+#include <ostream> // for ostream, operator<<
+#include <type_traits> // for enable_if_t
// Define all PCL point types
#define PCL_POINT_TYPES \
}
inline PointXYZRGBNormal (float _curvature = 0.f):
- PointXYZRGBNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {}
+ PointXYZRGBNormal (0.f, 0.f, 0.f, 0, 0, 0, 0.f, 0.f, 0.f, _curvature) {}
inline PointXYZRGBNormal (float _x, float _y, float _z):
PointXYZRGBNormal (_x, _y, _z, 0, 0, 0) {}
curvature = p.curvature;
intensity = p.intensity;
}
-
+
inline PointXYZINormal (float _intensity = 0.f): PointXYZINormal (0.f, 0.f, 0.f, _intensity) {}
-
+
inline PointXYZINormal (float _x, float _y, float _z, float _intensity = 0.f):
PointXYZINormal (_x, _y, _z, _intensity, 0.f, 0.f, 0.f) {}
}
inline PointXYZLNormal (std::uint32_t _label = 0): PointXYZLNormal (0.f, 0.f, 0.f, _label) {}
-
+
inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label = 0.f):
PointXYZLNormal (_x, _y, _z, _label, 0.f, 0.f, 0.f) {}
inline PointWithViewpoint (): PointWithViewpoint (0.f, 0.f, 0.f) {}
- PCL_DEPRECATED("Use ctor accepting all position (x, y, z) data")
+ PCL_DEPRECATED(1, 12, "Use ctor accepting all position (x, y, z) data")
inline PointWithViewpoint (float _x, float _y = 0.f):
PointWithViewpoint (_x, _y, 0.f) {}
- PCL_DEPRECATED("Use ctor accepting all viewpoint (vp_x, vp_y, vp_z) data")
+ PCL_DEPRECATED(1, 12, "Use ctor accepting all viewpoint (vp_x, vp_y, vp_z) data")
inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y = 0.f):
PointWithViewpoint (_x, _y, _z, _vp_x, _vp_y, 0.f) {}
-
+
inline PointWithViewpoint (float _x, float _y, float _z): PointWithViewpoint (_x, _y, _z, 0.f, 0.f, 0.f) {}
inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y, float _vp_z)
inline PrincipalRadiiRSD () = default;
inline PrincipalRadiiRSD (float _r_min, float _r_max): r_min (_r_min), r_max (_r_max) {}
-
+
friend std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p);
};
inline ReferenceFrame ()
{
- std::fill_n(x_axis, 3, 0);
- std::fill_n(y_axis, 3, 0);
- std::fill_n(z_axis, 3, 0);
+ std::fill_n(x_axis, 3, 0.f);
+ std::fill_n(y_axis, 3, 0.f);
+ std::fill_n(z_axis, 3, 0.f);
}
// @TODO: add other ctors
*/
struct BorderDescription
{
- int x = 0.f, y = 0.f;
+ int x = 0, y = 0;
BorderTraits traits;
//std::vector<const BorderDescription*> neighbors;
inline PointWithScale (): PointWithScale (0.f, 0.f, 0.f) {}
- inline PointWithScale (float _x, float _y, float _z, float _scale = 1.f,
+ inline PointWithScale (float _x, float _y, float _z, float _scale = 1.f,
float _angle = -1.f, float _response = 0.f, int _octave = 0)
{
x = _x; y = _y; z = _z;
friend std::ostream& operator << (std::ostream& os, const PointDEM& p);
};
- template <int N> std::ostream&
+ template <int N> std::ostream&
operator << (std::ostream& os, const Histogram<N>& p)
{
// make constexpr
}
return (os);
}
-} // End namespace
+} // namespace pcl
+
+// Register point structs and wrappers
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_RGB,
+ (std::uint32_t, rgba, rgba)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::RGB, pcl::_RGB)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity,
+ (float, intensity, intensity)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity, pcl::_Intensity)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity8u,
+ (std::uint8_t, intensity, intensity)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity8u, pcl::_Intensity8u)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity32u,
+ (std::uint32_t, intensity, intensity)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity32u, pcl::_Intensity32u)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZ,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZ, pcl::_PointXYZ)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBA,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (std::uint32_t, rgba, rgba)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBA, pcl::_PointXYZRGBA)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGB,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, rgb, rgb)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGB, pcl::_PointXYZRGB)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBL,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (std::uint32_t, rgba, rgba)
+ (std::uint32_t, label, label)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBL, pcl::_PointXYZRGBL)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZHSV,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, h, h)
+ (float, s, s)
+ (float, v, v)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZHSV, pcl::_PointXYZHSV)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXY,
+ (float, x, x)
+ (float, y, y)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointUV,
+ (float, u, u)
+ (float, v, v)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::InterestPoint,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, strength, strength)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZI,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, intensity, intensity)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZI, pcl::_PointXYZI)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZL,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (std::uint32_t, label, label)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Label,
+ (std::uint32_t, label, label)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Normal,
+ (float, normal_x, normal_x)
+ (float, normal_y, normal_y)
+ (float, normal_z, normal_z)
+ (float, curvature, curvature)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Normal, pcl::_Normal)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Axis,
+ (float, normal_x, normal_x)
+ (float, normal_y, normal_y)
+ (float, normal_z, normal_z)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Axis, pcl::_Axis)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointNormal,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, normal_x, normal_x)
+ (float, normal_y, normal_y)
+ (float, normal_z, normal_z)
+ (float, curvature, curvature)
+)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBNormal,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, rgb, rgb)
+ (float, normal_x, normal_x)
+ (float, normal_y, normal_y)
+ (float, normal_z, normal_z)
+ (float, curvature, curvature)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBNormal, pcl::_PointXYZRGBNormal)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZINormal,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, intensity, intensity)
+ (float, normal_x, normal_x)
+ (float, normal_y, normal_y)
+ (float, normal_z, normal_z)
+ (float, curvature, curvature)
+)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZLNormal,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (std::uint32_t, label, label)
+ (float, normal_x, normal_x)
+ (float, normal_y, normal_y)
+ (float, normal_z, normal_z)
+ (float, curvature, curvature)
+)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithRange,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, range, range)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointWithViewpoint,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, vp_x, vp_x)
+ (float, vp_y, vp_y)
+ (float, vp_z, vp_z)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointWithViewpoint, pcl::_PointWithViewpoint)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::MomentInvariants,
+ (float, j1, j1)
+ (float, j2, j2)
+ (float, j3, j3)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PrincipalRadiiRSD,
+ (float, r_min, r_min)
+ (float, r_max, r_max)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Boundary,
+ (std::uint8_t, boundary_point, boundary_point)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PrincipalCurvatures,
+ (float, principal_curvature_x, principal_curvature_x)
+ (float, principal_curvature_y, principal_curvature_y)
+ (float, principal_curvature_z, principal_curvature_z)
+ (float, pc1, pc1)
+ (float, pc2, pc2)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PFHSignature125,
+ (float[125], histogram, pfh)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PFHRGBSignature250,
+ (float[250], histogram, pfhrgb)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFSignature,
+ (float, f1, f1)
+ (float, f2, f2)
+ (float, f3, f3)
+ (float, f4, f4)
+ (float, alpha_m, alpha_m)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature,
+ (float, f1, f1)
+ (float, f2, f2)
+ (float, f3, f3)
+ (float, f4, f4)
+ (float, f5, f5)
+ (float, f6, f6)
+ (float, f7, f7)
+ (float, f8, f8)
+ (float, f9, f9)
+ (float, f10, f10)
+ (float, alpha_m, alpha_m)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFRGBSignature,
+ (float, f1, f1)
+ (float, f2, f2)
+ (float, f3, f3)
+ (float, f4, f4)
+ (float, r_ratio, r_ratio)
+ (float, g_ratio, g_ratio)
+ (float, b_ratio, b_ratio)
+ (float, alpha_m, alpha_m)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::NormalBasedSignature12,
+ (float[12], values, values)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ShapeContext1980,
+ (float[1980], descriptor, shape_context)
+ (float[9], rf, rf)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::UniqueShapeContext1960,
+ (float[1960], descriptor, shape_context)
+ (float[9], rf, rf)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::SHOT352,
+ (float[352], descriptor, shot)
+ (float[9], rf, rf)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::SHOT1344,
+ (float[1344], descriptor, shot)
+ (float[9], rf, rf)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::FPFHSignature33,
+ (float[33], histogram, fpfh)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::BRISKSignature512,
+ (float, scale, brisk_scale)
+ (float, orientation, brisk_orientation)
+ (unsigned char[64], descriptor, brisk_descriptor512)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::VFHSignature308,
+ (float[308], histogram, vfh)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::GRSDSignature21,
+ (float[21], histogram, grsd)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ESFSignature640,
+ (float[640], histogram, esf)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature512,
+ (float[512], histogram, gasd)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature984,
+ (float[984], histogram, gasd)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature7992,
+ (float[7992], histogram, gasd)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Narf36,
+ (float[36], descriptor, descriptor)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::GFPFHSignature16,
+ (float[16], histogram, gfpfh)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::IntensityGradient,
+ (float, gradient_x, gradient_x)
+ (float, gradient_y, gradient_y)
+ (float, gradient_z, gradient_z)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithScale,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, scale, scale)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointSurfel,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, normal_x, normal_x)
+ (float, normal_y, normal_y)
+ (float, normal_z, normal_z)
+ (std::uint32_t, rgba, rgba)
+ (float, radius, radius)
+ (float, confidence, confidence)
+ (float, curvature, curvature)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_ReferenceFrame,
+ (float[3], x_axis, x_axis)
+ (float[3], y_axis, y_axis)
+ (float[3], z_axis, z_axis)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::ReferenceFrame, pcl::_ReferenceFrame)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointDEM,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, intensity, intensity)
+ (float, intensity_variance, intensity_variance)
+ (float, height_variance, height_variance)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointDEM, pcl::_PointDEM)
+namespace pcl
+{
+
+// Allow float 'rgb' data to match to the newer uint32 'rgba' tag. This is so
+// you can load old 'rgb' PCD files into e.g. a PointCloud<PointXYZRGBA>.
+template<typename PointT>
+struct FieldMatches<PointT, ::pcl::fields::rgba>
+{
+ bool operator() (const pcl::PCLPointField& field)
+ {
+ if (field.name == "rgb")
+ {
+ // For fixing the alpha value bug #1141, the rgb field can also match
+ // uint32.
+ return ((field.datatype == pcl::PCLPointField::FLOAT32 ||
+ field.datatype == pcl::PCLPointField::UINT32) &&
+ field.count == 1);
+ }
+ else
+ {
+ return (field.name == traits::name<PointT, fields::rgba>::value &&
+ field.datatype == traits::datatype<PointT, fields::rgba>::value &&
+ field.count == traits::datatype<PointT, fields::rgba>::size);
+ }
+ }
+};
+template<typename PointT>
+struct FieldMatches<PointT, fields::rgb>
+{
+ bool operator() (const pcl::PCLPointField& field)
+ {
+ if (field.name == "rgba")
+ {
+ return (field.datatype == pcl::PCLPointField::UINT32 &&
+ field.count == 1);
+ }
+ else
+ {
+ // For fixing the alpha value bug #1141, rgb can also match uint32
+ return (field.name == traits::name<PointT, fields::rgb>::value &&
+ (field.datatype == traits::datatype<PointT, fields::rgb>::value ||
+ field.datatype == pcl::PCLPointField::UINT32) &&
+ field.count == traits::datatype<PointT, fields::rgb>::size);
+ }
+ }
+};
+
+
+// We're doing a lot of black magic with Boost here, so disable warnings in Maintainer mode, as we will never
+// be able to fix them anyway
+#if defined _MSC_VER
+ #pragma warning(disable: 4201)
#endif
+
+namespace traits
+{
+
+ /** \brief Metafunction to check if a given point type has a given field.
+ *
+ * Example usage at run-time:
+ *
+ * \code
+ * bool curvature_available = pcl::traits::has_field<PointT, pcl::fields::curvature>::value;
+ * \endcode
+ *
+ * Example usage at compile-time:
+ *
+ * \code
+ * BOOST_MPL_ASSERT_MSG ((pcl::traits::has_field<PointT, pcl::fields::label>::value),
+ * POINT_TYPE_SHOULD_HAVE_LABEL_FIELD,
+ * (PointT));
+ * \endcode
+ */
+ template <typename PointT, typename Field>
+ struct has_field : boost::mpl::contains<typename pcl::traits::fieldList<PointT>::type, Field>::type
+ { };
+
+ /** Metafunction to check if a given point type has all given fields. */
+ template <typename PointT, typename Field>
+ struct has_all_fields : boost::mpl::fold<Field,
+ boost::mpl::bool_<true>,
+ boost::mpl::and_<boost::mpl::_1,
+ has_field<PointT, boost::mpl::_2> > >::type
+ { };
+
+ /** Metafunction to check if a given point type has any of the given fields. */
+ template <typename PointT, typename Field>
+ struct has_any_field : boost::mpl::fold<Field,
+ boost::mpl::bool_<false>,
+ boost::mpl::or_<boost::mpl::_1,
+ has_field<PointT, boost::mpl::_2> > >::type
+ { };
+
+ /** \brief Traits defined for ease of use with fields already registered before
+ *
+ * has_<fields to be detected>: struct with `value` datamember defined at compiletime
+ * has_<fields to be detected>_v: constexpr boolean
+ * Has<Fields to be detected>: concept modelling name alias for `enable_if`
+ */
+
+ /** Metafunction to check if a given point type has x and y fields. */
+ template <typename PointT>
+ struct has_xy : has_all_fields<PointT, boost::mpl::vector<pcl::fields::x,
+ pcl::fields::y> >
+ { };
+
+ template <typename PointT>
+ constexpr auto has_xy_v = has_xy<PointT>::value;
+
+ template <typename PointT>
+ using HasXY = std::enable_if_t<has_xy_v<PointT>, bool>;
+
+ template <typename PointT>
+ using HasNoXY = std::enable_if_t<!has_xy_v<PointT>, bool>;
+
+ /** Metafunction to check if a given point type has x, y, and z fields. */
+ template <typename PointT>
+ struct has_xyz : has_all_fields<PointT, boost::mpl::vector<pcl::fields::x,
+ pcl::fields::y,
+ pcl::fields::z> >
+ { };
+
+ template <typename PointT>
+ constexpr auto has_xyz_v = has_xyz<PointT>::value;
+
+ template <typename PointT>
+ using HasXYZ = std::enable_if_t<has_xyz_v<PointT>, bool>;
+
+ template <typename PointT>
+ using HasNoXYZ = std::enable_if_t<!has_xyz_v<PointT>, bool>;
+
+ /** Metafunction to check if a given point type has normal_x, normal_y, and
+ * normal_z fields. */
+ template <typename PointT>
+ struct has_normal : has_all_fields<PointT, boost::mpl::vector<pcl::fields::normal_x,
+ pcl::fields::normal_y,
+ pcl::fields::normal_z> >
+ { };
+
+ template <typename PointT>
+ constexpr auto has_normal_v = has_normal<PointT>::value;
+
+ template <typename PointT>
+ using HasNormal = std::enable_if_t<has_normal_v<PointT>, bool>;
+
+ template <typename PointT>
+ using HasNoNormal = std::enable_if_t<!has_normal_v<PointT>, bool>;
+
+ /** Metafunction to check if a given point type has curvature field. */
+ template <typename PointT>
+ struct has_curvature : has_field<PointT, pcl::fields::curvature>
+ { };
+
+ template <typename PointT>
+ constexpr auto has_curvature_v = has_curvature<PointT>::value;
+
+ template <typename PointT>
+ using HasCurvature = std::enable_if_t<has_curvature_v<PointT>, bool>;
+
+ template <typename PointT>
+ using HasNoCurvature = std::enable_if_t<!has_curvature_v<PointT>, bool>;
+
+ /** Metafunction to check if a given point type has intensity field. */
+ template <typename PointT>
+ struct has_intensity : has_field<PointT, pcl::fields::intensity>
+ { };
+
+ template <typename PointT>
+ constexpr auto has_intensity_v = has_intensity<PointT>::value;
+
+ template <typename PointT>
+ using HasIntensity = std::enable_if_t<has_intensity_v<PointT>, bool>;
+
+ template <typename PointT>
+ using HasNoIntensity = std::enable_if_t<!has_intensity_v<PointT>, bool>;
+
+ /** Metafunction to check if a given point type has either rgb or rgba field. */
+ template <typename PointT>
+ struct has_color : has_any_field<PointT, boost::mpl::vector<pcl::fields::rgb,
+ pcl::fields::rgba> >
+ { };
+
+ template <typename PointT>
+ constexpr auto has_color_v = has_color<PointT>::value;
+
+ template <typename PointT>
+ using HasColor = std::enable_if_t<has_color_v<PointT>, bool>;
+
+ template <typename PointT>
+ using HasNoColor = std::enable_if_t<!has_color_v<PointT>, bool>;
+
+ /** Metafunction to check if a given point type has label field. */
+ template <typename PointT>
+ struct has_label : has_field<PointT, pcl::fields::label>
+ { };
+
+ template <typename PointT>
+ constexpr auto has_label_v = has_label<PointT>::value;
+
+ template <typename PointT>
+ using HasLabel = std::enable_if_t<has_label_v<PointT>, bool>;
+
+ template <typename PointT>
+ using HasNoLabel = std::enable_if_t<!has_label_v<PointT>, bool>;
+}
+
+#if defined _MSC_VER
+ #pragma warning(default: 4201)
+#endif
+
+} // namespace pcl
+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *
*/
#pragma once
+PCL_DEPRECATED_HEADER(1, 13, "Use <pcl/memory.h> instead.")
-#include <type_traits>
-#include <utility>
-
-#include <boost/make_shared.hpp>
-#include <boost/shared_ptr.hpp>
-
-#include <pcl/point_traits.h>
-
-
-namespace pcl
-{
-
-#ifdef DOXYGEN_ONLY
-
-/**
- * \brief Returns a pcl::shared_ptr compliant with type T's allocation policy.
- *
- * boost::allocate_shared or boost::make_shared will be invoked in case T has or
- * doesn't have a custom allocator, respectively.
- *
- * \see pcl::has_custom_allocator, PCL_MAKE_ALIGNED_OPERATOR_NEW
- * \tparam T Type of the object to create a pcl::shared_ptr of
- * \tparam Args Types for the arguments to pcl::make_shared
- * \param args List of arguments with which an instance of T will be constructed
- * \return pcl::shared_ptr of an instance of type T
- */
-template<typename T, typename ... Args>
-shared_ptr<T> make_shared(Args&&... args);
-
-#else
-
-template<typename T, typename ... Args>
-std::enable_if_t<has_custom_allocator<T>::value, shared_ptr<T>> make_shared(Args&&... args)
-{
- return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args> (args)...);
-}
-
-template<typename T, typename ... Args>
-std::enable_if_t<!has_custom_allocator<T>::value, shared_ptr<T>> make_shared(Args&&... args)
-{
- return boost::make_shared<T>(std::forward<Args> (args)...);
-}
-
-#endif
+#include <pcl/memory.h>
-} // namespace pcl
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2019-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+
+/**
+ * \file pcl/memory.h
+ *
+ * \brief Defines functions, macros and traits for allocating and using memory.
+ * \ingroup common
+ */
+
+#include <pcl/type_traits.h> // for has_custom_allocator
+
+#include <Eigen/Core> // for EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+#include <memory> // for std::allocate_shared, std::dynamic_pointer_cast, std::make_shared, std::shared_ptr, std::static_pointer_cast, std::weak_ptr
+#include <type_traits> // for std::enable_if_t, std::false_type, std::true_type
+#include <utility> // for std::forward
+
+/**
+ * \brief Macro to signal a class requires a custom allocator
+ *
+ * It's an implementation detail to have pcl::has_custom_allocator work, a
+ * thin wrapper over Eigen's own macro
+ *
+ * \see pcl::has_custom_allocator, pcl::make_shared
+ * \ingroup common
+ */
+#define PCL_MAKE_ALIGNED_OPERATOR_NEW \
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
+ using _custom_allocator_type_trait = void;
+
+
+namespace pcl
+{
+/**
+ * \brief Force ADL for `shared_ptr`
+ *
+ * For ease of switching from boost::shared_ptr to std::shared_ptr
+ *
+ * \see pcl::make_shared
+ */
+using std::shared_ptr;
+
+/**
+ * \brief Force ADL for `weak_ptr`
+ *
+ * For ease of switching from boost::weak_ptr to std::weak_ptr
+ */
+using std::weak_ptr;
+
+/** ADL doesn't work until C++20 for dynamic_pointer_cast since it requires an explicit Tparam */
+using std::dynamic_pointer_cast;
+
+/** ADL doesn't work until C++20 for static_pointer_cast since it requires an explicit Tparam */
+using std::static_pointer_cast;
+
+#ifdef DOXYGEN_ONLY
+
+/**
+ * \brief Returns a pcl::shared_ptr compliant with type T's allocation policy.
+ *
+ * std::allocate_shared or std::make_shared will be invoked in case T has or
+ * doesn't have a custom allocator, respectively.
+ *
+ * \note In MSVC < 1915 (before version 15.8) alignment was incorrectly set at
+ * most at alignof(max_align_t). This bug was fixed in said version and is
+ * acknowledged by defining _ENABLE_EXTENDED_ALIGNED_STORAGE. See #3752.
+ *
+ * \see pcl::has_custom_allocator, PCL_MAKE_ALIGNED_OPERATOR_NEW
+ * \tparam T Type of the object to create a pcl::shared_ptr of
+ * \tparam Args Types for the arguments to pcl::make_shared
+ * \param args List of arguments with which an instance of T will be constructed
+ * \return pcl::shared_ptr of an instance of type T
+ */
+template<typename T, typename ... Args>
+shared_ptr<T> make_shared(Args&&... args);
+
+#else
+
+template<typename T, typename ... Args>
+std::enable_if_t<has_custom_allocator<T>::value, shared_ptr<T>> make_shared(Args&&... args)
+{
+ return std::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args> (args)...);
+}
+
+template<typename T, typename ... Args>
+std::enable_if_t<!has_custom_allocator<T>::value, shared_ptr<T>> make_shared(Args&&... args)
+{
+ return std::make_shared<T>(std::forward<Args> (args)...);
+}
+
+#endif
+}
#endif
// Include PCL macros such as PCL_ERROR, PCL_MAKE_ALIGNED_OPERATOR_NEW, etc
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
-#include <boost/shared_ptr.hpp>
#include <Eigen/StdVector>
#include <Eigen/Core>
#include <pcl/point_cloud.h>
#include <pcl/PointIndices.h>
#include <pcl/PCLPointCloud2.h>
+#include <pcl/types.h>
namespace pcl
{
// definitions used everywhere
- using Indices = std::vector<int>;
using IndicesPtr = shared_ptr<Indices>;
using IndicesConstPtr = shared_ptr<const Indices>;
PCLBase (const PCLBase& base);
/** \brief Destructor. */
- virtual ~PCLBase ()
- {
- input_.reset ();
- indices_.reset ();
- }
+ virtual ~PCLBase () = default;
/** \brief Provide a pointer to the input dataset
* \param[in] cloud the const boost shared pointer to a PointCloud message
PCLBase ();
/** \brief destructor. */
- virtual ~PCLBase()
- {
- input_.reset ();
- indices_.reset ();
- }
+ virtual ~PCLBase () = default;
/** \brief Provide a pointer to the input dataset
* \param cloud the const boost shared pointer to a PointCloud message
#endif
#ifndef _USE_MATH_DEFINES
-#define _USE_MATH_DEFINES
+ #define _USE_MATH_DEFINES
#endif
#include <cmath>
#include <cstdarg>
#include <cstdint>
#include <iostream>
-#include <boost/cstdint.hpp>
-#include <boost/smart_ptr/shared_ptr.hpp>
-
-//Eigen has an enum that clashes with X11 Success define, which is ultimately included by pcl
-#ifdef Success
- #undef Success
+// We need to check for GCC version, because GCC releases before 9 were implementing an
+// OpenMP 3.1 data sharing rule, even OpenMP 4 is supported, so a plain OpenMP version 4 check
+// isn't enough (see https://www.gnu.org/software/gcc/gcc-9/porting_to.html#ompdatasharing)
+#if (defined _OPENMP && (_OPENMP <= 201307)) || (defined __GNUC__ && (__GNUC__ >= 6 && __GNUC__ < 9))
+ #define OPENMP_LEGACY_CONST_DATA_SHARING_RULE 1
+#else
+ #define OPENMP_LEGACY_CONST_DATA_SHARING_RULE 0
#endif
-#include <Eigen/Core>
#include <pcl/pcl_config.h>
-// It seems that __has_cpp_attribute doesn't work correctly
-// when compiling with some versions of nvcc so we
-// additionally check if nvcc is used before setting the
-// PCL_DEPRECATED macro to [[deprecated]].
-#if defined(__has_cpp_attribute) && __has_cpp_attribute(deprecated) && !defined(__CUDACC__)
- #define PCL_DEPRECATED(message) [[deprecated(message)]]
-#elif defined(__GNUC__) || defined(__clang__)
- #define PCL_DEPRECATED(message) __attribute__((deprecated(message)))
-#elif defined(_MSC_VER)
- // Until Visual Studio 2013 you had to use __declspec(deprecated).
- // However, we decided to ignore the deprecation for these version because
- // of simplicity reasons. See PR #3634 for the details.
- #define PCL_DEPRECATED(message)
+#include <boost/preprocessor/comparison/equal.hpp>
+#include <boost/preprocessor/comparison/less.hpp>
+#include <boost/preprocessor/control/if.hpp>
+#include <boost/preprocessor/stringize.hpp>
+
+// MSVC < 2019 have issues:
+// * can't short-circuiting logic in macros
+// * don't define standard macros
+// => this leads to annyoing C4067 warnings (see https://developercommunity.visualstudio.com/content/problem/327796/-has-cpp-attribute-emits-warning-is-wrong-highligh.html)
+#if defined(_MSC_VER)
+ // nvcc on msvc can't work with [[deprecated]]
+ #if !defined(__CUDACC__)
+ #define _PCL_DEPRECATED_IMPL(Message) [[deprecated(Message)]]
+ #else
+ #define _PCL_DEPRECATED_IMPL(Message)
+ #endif
+#elif __has_cpp_attribute(deprecated)
+ #define _PCL_DEPRECATED_IMPL(Message) [[deprecated(Message)]]
#else
- #warning "You need to implement PCL_DEPRECATED for this compiler"
- #define PCL_DEPRECATED(message)
+ #warning "You need to implement _PCL_DEPRECATED_IMPL for this compiler"
+ #define _PCL_DEPRECATED_IMPL(Message)
#endif
-namespace pcl
-{
- /**
- * \brief Alias for boost::shared_ptr
- *
- * For ease of switching from boost::shared_ptr to std::shared_ptr
- *
- * \see pcl::make_shared
- * \tparam T Type of the object stored inside the shared_ptr
- */
- template <typename T>
- using shared_ptr = boost::shared_ptr<T>;
-
- using uint8_t PCL_DEPRECATED("use std::uint8_t instead of pcl::uint8_t") = std::uint8_t;
- using int8_t PCL_DEPRECATED("use std::int8_t instead of pcl::int8_t") = std::int8_t;
- using uint16_t PCL_DEPRECATED("use std::uint16_t instead of pcl::uint16_t") = std::uint16_t;
- using int16_t PCL_DEPRECATED("use std::uint16_t instead of pcl::int16_t") = std::int16_t;
- using uint32_t PCL_DEPRECATED("use std::uint32_t instead of pcl::uint32_t") = std::uint32_t;
- using int32_t PCL_DEPRECATED("use std::int32_t instead of pcl::int32_t") = std::int32_t;
- using uint64_t PCL_DEPRECATED("use std::uint64_t instead of pcl::uint64_t") = std::uint64_t;
- using int64_t PCL_DEPRECATED("use std::int64_t instead of pcl::int64_t") = std::int64_t;
- using int_fast16_t PCL_DEPRECATED("use std::int_fast16_t instead of pcl::int_fast16_t") = std::int_fast16_t;
-}
+// Macro for pragma operator
+#if (defined (__GNUC__) || defined(__clang__))
+ #define PCL_PRAGMA(x) _Pragma (#x)
+#elif _MSC_VER
+ #define PCL_PRAGMA(x) __pragma (#x)
+#else
+ #define PCL_PRAGMA
+#endif
+
+// Macro for emitting pragma warning for deprecated headers
+#if (defined (__GNUC__) || defined(__clang__))
+ #define _PCL_DEPRECATED_HEADER_IMPL(Message) PCL_PRAGMA (GCC warning Message)
+#elif _MSC_VER
+ #define _PCL_DEPRECATED_HEADER_IMPL(Message) PCL_PRAGMA (warning (Message))
+#else
+ #warning "You need to implement _PCL_DEPRECATED_HEADER_IMPL for this compiler"
+ #define _PCL_DEPRECATED_HEADER_IMPL(Message)
+#endif
-#if defined _WIN32 && defined _MSC_VER
+/**
+ * \brief A handy way to inform the user of the removal deadline
+ */
+#define _PCL_PREPARE_REMOVAL_MESSAGE(Major, Minor, Msg) \
+ Msg " (It will be removed in PCL " BOOST_PP_STRINGIZE(Major.Minor) ")"
+/**
+ * \brief Tests for Minor < PCL_MINOR_VERSION
+ */
+#define _PCL_COMPAT_MINOR_VERSION(Minor, IfPass, IfFail) \
+ BOOST_PP_IF(BOOST_PP_LESS(PCL_MINOR_VERSION, Minor), IfPass, IfFail)
+
+/**
+ * \brief Tests for Major == PCL_MAJOR_VERSION
+ */
+#define _PCL_COMPAT_MAJOR_VERSION(Major, IfPass, IfFail) \
+ BOOST_PP_IF(BOOST_PP_EQUAL(PCL_MAJOR_VERSION, Major), IfPass, IfFail)
+
+/**
+ * \brief macro for compatibility across compilers and help remove old deprecated
+ * items for the Major.Minor release
+ *
+ * \details compiler errors of `unneeded_deprecation` and `major_version_mismatch`
+ * are hints to the developer that those items can be safely removed.
+ * Behavior of PCL_DEPRECATED(1, 99, "Not needed anymore")
+ * * till PCL 1.98: "Not needed anymore (It will be removed in PCL 1.99)"
+ * * PCL 1.99 onwards: compiler error with "unneeded_deprecation"
+ * * PCL 2.0 onwards: compiler error with "major_version_mismatch"
+ */
+#define PCL_DEPRECATED(Major, Minor, Message) \
+ _PCL_COMPAT_MAJOR_VERSION( \
+ Major, \
+ _PCL_COMPAT_MINOR_VERSION( \
+ Minor, \
+ _PCL_DEPRECATED_IMPL(_PCL_PREPARE_REMOVAL_MESSAGE(Major, Minor, Message)), \
+ unneeded_deprecation), \
+ major_version_mismatch)
+
+/**
+ * \brief macro for compatibility across compilers and help remove old deprecated
+ * headers for the Major.Minor release
+ *
+ * \details compiler errors of `unneeded_header` and `major_version_mismatch`
+ * are hints to the developer that those items can be safely removed.
+ * Behavior of PCL_DEPRECATED_HEADER(1, 99, "Use file <newfile.h> instead.")
+ * * till PCL 1.98: "This header is deprecated. Use file <newfile.h> instead. (It will be removed in PCL 1.99)"
+ * * PCL 1.99 onwards: compiler error with "unneeded_header"
+ * * PCL 2.0 onwards: compiler error with "major_version_mismatch"
+ */
+#define PCL_DEPRECATED_HEADER(Major, Minor, Message) \
+ _PCL_COMPAT_MAJOR_VERSION( \
+ Major, \
+ _PCL_COMPAT_MINOR_VERSION( \
+ Minor, \
+ _PCL_DEPRECATED_HEADER_IMPL(_PCL_PREPARE_REMOVAL_MESSAGE( \
+ Major, \
+ Minor, \
+ "This header is deprecated. " Message)), \
+ unneeded_header), \
+ major_version_mismatch)
+
+#if defined _WIN32
// Define math constants, without including math.h, to prevent polluting global namespace with old math methods
// Copied from math.h
-#ifndef _MATH_DEFINES_DEFINED
+// Check for M_2_SQRTPI since the cmath header on mingw-w64 doesn't seem to define
+// _MATH_DEFINES_DEFINED when included with _USE_MATH_DEFINES
+#if !defined _MATH_DEFINES_DEFINED && !defined M_2_SQRTPI
#define _MATH_DEFINES_DEFINED
#define M_E 2.71828182845904523536 // e
#define M_SQRT1_2 0.707106781186547524401 // 1/sqrt(2)
#endif
-// Stupid. This should be removed when all the PCL dependencies have min/max fixed.
-#ifndef NOMINMAX
-# define NOMINMAX
-#endif
-
-# define __PRETTY_FUNCTION__ __FUNCTION__
-# define __func__ __FUNCTION__
+#if defined _MSC_VER
+ // Stupid. This should be removed when all the PCL dependencies have min/max fixed.
+ #ifndef NOMINMAX
+ #define NOMINMAX
+ #endif
-#endif //defined _WIN32 && defined _MSC_VER
+ #define __PRETTY_FUNCTION__ __FUNCTION__
+ #define __func__ __FUNCTION__
+#endif
+#endif // defined _WIN32
template<typename T>
-PCL_DEPRECATED("use std::isnan instead of pcl_isnan")
+PCL_DEPRECATED(1, 12, "use std::isnan instead of pcl_isnan")
bool pcl_isnan (T&& x) { return std::isnan (std::forward<T> (x)); }
template<typename T>
-PCL_DEPRECATED("use std::isfinite instead of pcl_isfinite")
+PCL_DEPRECATED(1, 12, "use std::isfinite instead of pcl_isfinite")
bool pcl_isfinite (T&& x) { return std::isfinite (std::forward<T> (x)); }
template<typename T>
-PCL_DEPRECATED("use std::isinf instead of pcl_isinf")
+PCL_DEPRECATED(1, 12, "use std::isinf instead of pcl_isinf")
bool pcl_isinf (T&& x) { return std::isinf (std::forward<T> (x)); }
#define PCLAPI(rettype) PCL_EXTERN_C PCL_EXPORTS rettype PCL_CDECL
#endif
-// Macro for pragma operator
-#if (defined (__GNUC__) || defined(__clang__))
- #define PCL_PRAGMA(x) _Pragma (#x)
-#elif _MSC_VER
- #define PCL_PRAGMA(x) __pragma (#x)
-#else
- #define PCL_PRAGMA
-#endif
-
-// Macro for emitting pragma warning
-#if (defined (__GNUC__) || defined(__clang__))
- #define PCL_PRAGMA_WARNING(x) PCL_PRAGMA (GCC warning x)
-#elif _MSC_VER
- #define PCL_PRAGMA_WARNING(x) PCL_PRAGMA (warning (x))
-#else
- #define PCL_PRAGMA_WARNING
-#endif
-
//for clang cf. http://clang.llvm.org/docs/LanguageExtensions.html
#ifndef __has_extension
#define __has_extension(x) 0 // Compatibility with pre-3.0 compilers.
#endif
}
-/**
- * \brief Macro to signal a class requires a custom allocator
- *
- * It's an implementation detail to have pcl::has_custom_allocator work, a
- * thin wrapper over Eigen's own macro
- *
- * \see pcl::has_custom_allocator, pcl::make_shared
- * \ingroup common
- */
-#define PCL_MAKE_ALIGNED_OPERATOR_NEW \
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
- using _custom_allocator_type_trait = void;
-
/**
* \brief Macro to add a no-op or a fallthrough attribute based on compiler feature
*
#include <Eigen/Geometry>
#include <pcl/PCLHeader.h>
#include <pcl/exceptions.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
-#include <pcl/point_traits.h>
-
-#include <pcl/make_shared.h>
+#include <pcl/type_traits.h>
#include <algorithm>
#include <utility>
namespace detail
{
template <typename PointT>
- PCL_DEPRECATED("use createMapping() instead")
+ PCL_DEPRECATED(1, 12, "use createMapping() instead")
shared_ptr<pcl::MsgFieldMap>&
getMapping (pcl::PointCloud<PointT>& p);
} // namespace detail
*/
PointCloud () = default;
+ /** \brief Copy constructor.
+ * \param[in] pc the cloud to copy into this
+ * \todo Erase once mapping_ is removed.
+ */
+ // Ignore unknown pragma warning on MSVC (4996)
+ #pragma warning(push)
+ #pragma warning(disable: 4068)
+ // Ignore deprecated warning on clang compilers
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wdeprecated-declarations"
+ PointCloud (const PointCloud<PointT> &pc) = default;
+ #pragma clang diagnostic pop
+ #pragma warning(pop)
+
/** \brief Copy constructor from point cloud subset
* \param[in] pc the cloud to copy into this
* \param[in] indices the subset to copy
}
/** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
- * \anchor getMatrixXfMap
* \note This method is for advanced users only! Use with care!
*
* \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL
}
/** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
- * \anchor getMatrixXfMap
* \note This method is for advanced users only! Use with care!
*
* \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL
return (Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(const_cast<PointT*>(&points[0]))+offset, dim, points.size (), Eigen::OuterStride<> (stride)));
}
- /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
- * \note This method is for advanced users only! Use with care!
- * \attention PointT types are most of the time aligned, so the offsets are not continuous!
- * See \ref getMatrixXfMap for more information.
- */
+ /**
+ * \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
+ * \note This method is for advanced users only! Use with care!
+ * \attention PointT types are most of the time aligned, so the offsets are not continuous!
+ * \overload Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > pcl::PointCloud::getMatrixXfMap ()
+ */
inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
getMatrixXfMap ()
{
return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0));
}
- /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
- * \note This method is for advanced users only! Use with care!
- * \attention PointT types are most of the time aligned, so the offsets are not continuous!
- * See \ref getMatrixXfMap for more information.
- */
+ /**
+ * \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
+ * \note This method is for advanced users only! Use with care!
+ * \attention PointT types are most of the time aligned, so the offsets are not continuous!
+ * \overload const Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > pcl::PointCloud::getMatrixXfMap () const
+ */
inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
getMatrixXfMap () const
{
makeShared () const { return Ptr (new PointCloud<PointT> (*this)); }
protected:
- // This is motivated by ROS integration. Users should not need to access mapping_.
- PCL_DEPRECATED("rewrite your code to avoid using this protected field") shared_ptr<MsgFieldMap> mapping_;
+ /** \brief This is motivated by ROS integration. Users should not need to access mapping_.
+ * \todo Once mapping_ is removed, erase the explicitly defined copy constructor in PointCloud.
+ */
+ PCL_DEPRECATED(1, 12, "rewrite your code to avoid using this protected field") shared_ptr<MsgFieldMap> mapping_;
friend shared_ptr<MsgFieldMap>& detail::getMapping<PointT>(pcl::PointCloud<PointT> &p);
#include <vector>
#include <pcl/point_types.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/for_each_type.h>
#pragma once
-#ifdef __GNUC__
-#pragma GCC system_header
-#endif
+PCL_DEPRECATED_HEADER(1, 13, "Use <pcl/type_traits.h> instead.")
-#include "pcl/pcl_macros.h"
-
-#include <pcl/PCLPointField.h>
-#include <boost/mpl/assert.hpp>
-
-// This is required for the workaround at line 109
-#ifdef _MSC_VER
-#include <Eigen/Core>
-#include <Eigen/src/StlSupport/details.h>
-#endif
-
-#include <type_traits>
-
-namespace pcl
-{
- namespace deprecated
- {
- /** \class DeprecatedType
- * \brief A dummy type to aid in template parameter deprecation
- */
- struct T {};
- }
-
- namespace fields
- {
- // Tag types get put in this namespace
- }
-
- namespace traits
- {
- // Metafunction to return enum value representing a type
- template<typename T> struct asEnum {};
- template<> struct asEnum<std::int8_t> { static const std::uint8_t value = pcl::PCLPointField::INT8; };
- template<> struct asEnum<std::uint8_t> { static const std::uint8_t value = pcl::PCLPointField::UINT8; };
- template<> struct asEnum<std::int16_t> { static const std::uint8_t value = pcl::PCLPointField::INT16; };
- template<> struct asEnum<std::uint16_t> { static const std::uint8_t value = pcl::PCLPointField::UINT16; };
- template<> struct asEnum<std::int32_t> { static const std::uint8_t value = pcl::PCLPointField::INT32; };
- template<> struct asEnum<std::uint32_t> { static const std::uint8_t value = pcl::PCLPointField::UINT32; };
- template<> struct asEnum<float> { static const std::uint8_t value = pcl::PCLPointField::FLOAT32; };
- template<> struct asEnum<double> { static const std::uint8_t value = pcl::PCLPointField::FLOAT64; };
-
- // Metafunction to return type of enum value
- template<int> struct asType {};
- template<> struct asType<pcl::PCLPointField::INT8> { using type = std::int8_t; };
- template<> struct asType<pcl::PCLPointField::UINT8> { using type = std::uint8_t; };
- template<> struct asType<pcl::PCLPointField::INT16> { using type = std::int16_t; };
- template<> struct asType<pcl::PCLPointField::UINT16> { using type = std::uint16_t; };
- template<> struct asType<pcl::PCLPointField::INT32> { using type = std::int32_t; };
- template<> struct asType<pcl::PCLPointField::UINT32> { using type = std::uint32_t; };
- template<> struct asType<pcl::PCLPointField::FLOAT32> { using type = float; };
- template<> struct asType<pcl::PCLPointField::FLOAT64> { using type = double; };
-
- // Metafunction to decompose a type (possibly of array of any number of dimensions) into
- // its scalar type and total number of elements.
- template<typename T> struct decomposeArray
- {
- using type = std::remove_all_extents_t<T>;
- static const std::uint32_t value = sizeof (T) / sizeof (type);
- };
-
- // For non-POD point types, this is specialized to return the corresponding POD type.
- template<typename PointT>
- struct POD
- {
- using type = PointT;
- };
-
-#ifdef _MSC_VER
-
- /* Sometimes when calling functions like `copyPoint()` or `copyPointCloud`
- * without explicitly specifying point types, MSVC deduces them to be e.g.
- * `Eigen::internal::workaround_msvc_stl_support<pcl::PointXYZ>` instead of
- * plain `pcl::PointXYZ`. Subsequently these types are passed to meta-
- * functions like `has_field` or `fieldList` and make them choke. This hack
- * makes use of the fact that internally `fieldList` always applies `POD` to
- * its argument type. This specialization therefore allows to unwrap the
- * contained point type. */
- template<typename PointT>
- struct POD<Eigen::internal::workaround_msvc_stl_support<PointT> >
- {
- using type = PointT;
- };
-
-#endif
-
- // name
- /* This really only depends on Tag, but we go through some gymnastics to avoid ODR violations.
- We template it on the point type PointT to avoid ODR violations when registering multiple
- point types with shared tags.
- The dummy parameter is so we can partially specialize name on PointT and Tag but leave it
- templated on dummy. Each specialization declares a static char array containing the tag
- name. The definition of the static member would conflict when linking multiple translation
- units that include the point type registration. But when the static member definition is
- templated (on dummy), we sidestep the ODR issue.
- */
- template<class PointT, typename Tag, int dummy = 0>
- struct name : name<typename POD<PointT>::type, Tag, dummy>
- {
- // Contents of specialization:
- // static const char value[];
-
- // Avoid infinite compile-time recursion
- BOOST_MPL_ASSERT_MSG((!std::is_same<PointT, typename POD<PointT>::type>::value),
- POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
- };
-
- // offset
- template<class PointT, typename Tag>
- struct offset : offset<typename POD<PointT>::type, Tag>
- {
- // Contents of specialization:
- // static const std::size_t value;
-
- // Avoid infinite compile-time recursion
- BOOST_MPL_ASSERT_MSG((!std::is_same<PointT, typename POD<PointT>::type>::value),
- POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
- };
-
- // datatype
- template<class PointT, typename Tag>
- struct datatype : datatype<typename POD<PointT>::type, Tag>
- {
- // Contents of specialization:
- // using type = ...;
- // static const std::uint8_t value;
- // static const std::uint32_t size;
-
- // Avoid infinite compile-time recursion
- BOOST_MPL_ASSERT_MSG((!std::is_same<PointT, typename POD<PointT>::type>::value),
- POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
- };
-
- // fields
- template<typename PointT>
- struct fieldList : fieldList<typename POD<PointT>::type>
- {
- // Contents of specialization:
- // using type = boost::mpl::vector<...>;
-
- // Avoid infinite compile-time recursion
- BOOST_MPL_ASSERT_MSG((!std::is_same<PointT, typename POD<PointT>::type>::value),
- POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
- };
- } //namespace traits
-
- // Return true if the PCLPointField matches the expected name and data type.
- // Written as a struct to allow partially specializing on Tag.
- template<typename PointT, typename Tag>
- struct FieldMatches
- {
- bool operator() (const pcl::PCLPointField& field)
- {
- return (field.name == traits::name<PointT, Tag>::value &&
- field.datatype == traits::datatype<PointT, Tag>::value &&
- (field.count == traits::datatype<PointT, Tag>::size ||
- ((field.count == 0) && (traits::datatype<PointT, Tag>::size == 1 /* see bug #821 */))));
- }
- };
-
- /** \brief A helper functor that can copy a specific value if the given field exists.
- *
- * \note In order to actually copy the value an instance of this functor should be passed
- * to a pcl::for_each_type loop. See the example below.
- *
- * \code
- * PointInT p;
- * bool exists;
- * float value;
- * using FieldList = typename pcl::traits::fieldList<PointInT>::type;
- * pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<PointT, float> (p, "intensity", exists, value));
- * \endcode
- */
- template <typename PointInT, typename OutT>
- struct CopyIfFieldExists
- {
- using Pod = typename traits::POD<PointInT>::type;
-
- /** \brief Constructor.
- * \param[in] pt the input point
- * \param[in] field the name of the field
- * \param[out] exists set to true if the field exists, false otherwise
- * \param[out] value the copied field value
- */
- CopyIfFieldExists (const PointInT &pt,
- const std::string &field,
- bool &exists,
- OutT &value)
- : pt_ (reinterpret_cast<const Pod&>(pt)), name_ (field), exists_ (exists), value_ (value)
- {
- exists_ = false;
- }
-
- /** \brief Constructor.
- * \param[in] pt the input point
- * \param[in] field the name of the field
- * \param[out] value the copied field value
- */
- CopyIfFieldExists (const PointInT &pt,
- const std::string &field,
- OutT &value)
- : pt_ (reinterpret_cast<const Pod&>(pt)), name_ (field), exists_ (exists_tmp_), value_ (value)
- {
- }
-
- /** \brief Operator. Data copy happens here. */
- template <typename Key> inline void
- operator() ()
- {
- if (name_ == pcl::traits::name<PointInT, Key>::value)
- {
- exists_ = true;
- using T = typename pcl::traits::datatype<PointInT, Key>::type;
- const std::uint8_t* data_ptr = reinterpret_cast<const std::uint8_t*>(&pt_) + pcl::traits::offset<PointInT, Key>::value;
- value_ = static_cast<OutT> (*reinterpret_cast<const T*>(data_ptr));
- }
- }
-
- private:
- const Pod &pt_;
- const std::string &name_;
- bool &exists_;
- // Bogus entry
- bool exists_tmp_;
- OutT &value_;
- };
-
- /** \brief A helper functor that can set a specific value in a field if the field exists.
- *
- * \note In order to actually set the value an instance of this functor should be passed
- * to a pcl::for_each_type loop. See the example below.
- *
- * \code
- * PointT p;
- * using FieldList = typename pcl::traits::fieldList<PointT>::type;
- * pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<PointT, float> (p, "intensity", 42.0f));
- * \endcode
- */
- template <typename PointOutT, typename InT>
- struct SetIfFieldExists
- {
- using Pod = typename traits::POD<PointOutT>::type;
-
- /** \brief Constructor.
- * \param[in] pt the input point
- * \param[in] field the name of the field
- * \param[out] value the value to set
- */
- SetIfFieldExists (PointOutT &pt,
- const std::string &field,
- const InT &value)
- : pt_ (reinterpret_cast<Pod&>(pt)), name_ (field), value_ (value)
- {
- }
-
- /** \brief Operator. Data copy happens here. */
- template <typename Key> inline void
- operator() ()
- {
- if (name_ == pcl::traits::name<PointOutT, Key>::value)
- {
- using T = typename pcl::traits::datatype<PointOutT, Key>::type;
- std::uint8_t* data_ptr = reinterpret_cast<std::uint8_t*>(&pt_) + pcl::traits::offset<PointOutT, Key>::value;
- *reinterpret_cast<T*>(data_ptr) = static_cast<T> (value_);
- }
- }
-
- private:
- Pod &pt_;
- const std::string &name_;
- const InT &value_;
- };
-
- /** \brief Set the value at a specified field in a point
- * \param[out] pt the point to set the value to
- * \param[in] field_offset the offset of the field
- * \param[in] value the value to set
- */
- template <typename PointT, typename ValT> inline void
- setFieldValue (PointT &pt, std::size_t field_offset, const ValT &value)
- {
- std::uint8_t* data_ptr = reinterpret_cast<std::uint8_t*>(&pt) + field_offset;
- *reinterpret_cast<ValT*>(data_ptr) = value;
- }
-
- /** \brief Get the value at a specified field in a point
- * \param[in] pt the point to get the value from
- * \param[in] field_offset the offset of the field
- * \param[out] value the value to retrieve
- */
- template <typename PointT, typename ValT> inline void
- getFieldValue (const PointT &pt, std::size_t field_offset, ValT &value)
- {
- const std::uint8_t* data_ptr = reinterpret_cast<const std::uint8_t*>(&pt) + field_offset;
- value = *reinterpret_cast<const ValT*>(data_ptr);
- }
-
- template <typename ...> using void_t = void; // part of std in c++17
-
-#ifdef DOXYGEN_ONLY
-
- /**
- * \brief Tests at compile time if type T has a custom allocator
- *
- * \see pcl::make_shared, PCL_MAKE_ALIGNED_OPERATOR_NEW
- * \tparam T Type of the object to test
- */
- template <typename T> struct has_custom_allocator;
-
-#else
-
- template <typename, typename = void_t<>> struct has_custom_allocator : std::false_type {};
- template <typename T> struct has_custom_allocator<T, void_t<typename T::_custom_allocator_type_trait>> : std::true_type {};
-
-#endif
-}
+#include <pcl/type_traits.h>
#pragma once
-#include <pcl/pcl_macros.h>
#include <bitset>
-#include <pcl/register_point_struct.h>
-#include <boost/mpl/contains.hpp>
-#include <boost/mpl/fold.hpp>
-#include <boost/mpl/vector.hpp>
+
/**
* \file pcl/point_types.h
* \ingroup common
*/
-// We're doing a lot of black magic with Boost here, so disable warnings in Maintainer mode, as we will never
-// be able to fix them anyway
+// Allow nameless structs/unions
#if defined _MSC_VER
#pragma warning(disable: 4201)
#endif
-//#pragma warning(push, 1)
-#if defined __GNUC__
-# pragma GCC system_header
-#endif
/** @{*/
namespace pcl
* \ingroup common
*/
struct FPFHSignature33;
-
+
/** \brief Members: float vfh[308]
* \ingroup common
*/
struct VFHSignature308;
-
+
/** \brief Members: float grsd[21]
* \ingroup common
*/
struct GRSDSignature21;
-
+
/** \brief Members: float esf[640]
* \ingroup common
*/
* \ingroup common
*/
struct PointDEM;
-}
-
-/** @} */
-
-#include <pcl/impl/point_types.hpp> // Include struct definitions
-
-// ==============================
-// =====POINT_CLOUD_REGISTER=====
-// ==============================
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_RGB,
- (std::uint32_t, rgba, rgba)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::RGB, pcl::_RGB)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity,
- (float, intensity, intensity)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity, pcl::_Intensity)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity8u,
- (std::uint8_t, intensity, intensity)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity8u, pcl::_Intensity8u)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity32u,
- (std::uint32_t, intensity, intensity)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity32u, pcl::_Intensity32u)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZ,
- (float, x, x)
- (float, y, y)
- (float, z, z)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZ, pcl::_PointXYZ)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBA,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (std::uint32_t, rgba, rgba)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBA, pcl::_PointXYZRGBA)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGB,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, rgb, rgb)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGB, pcl::_PointXYZRGB)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBL,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (std::uint32_t, rgba, rgba)
- (std::uint32_t, label, label)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBL, pcl::_PointXYZRGBL)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZHSV,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, h, h)
- (float, s, s)
- (float, v, v)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZHSV, pcl::_PointXYZHSV)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXY,
- (float, x, x)
- (float, y, y)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointUV,
- (float, u, u)
- (float, v, v)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::InterestPoint,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, strength, strength)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZI,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, intensity, intensity)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZI, pcl::_PointXYZI)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZL,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (std::uint32_t, label, label)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Label,
- (std::uint32_t, label, label)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Normal,
- (float, normal_x, normal_x)
- (float, normal_y, normal_y)
- (float, normal_z, normal_z)
- (float, curvature, curvature)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Normal, pcl::_Normal)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Axis,
- (float, normal_x, normal_x)
- (float, normal_y, normal_y)
- (float, normal_z, normal_z)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Axis, pcl::_Axis)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointNormal,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, normal_x, normal_x)
- (float, normal_y, normal_y)
- (float, normal_z, normal_z)
- (float, curvature, curvature)
-)
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBNormal,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, rgb, rgb)
- (float, normal_x, normal_x)
- (float, normal_y, normal_y)
- (float, normal_z, normal_z)
- (float, curvature, curvature)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBNormal, pcl::_PointXYZRGBNormal)
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZINormal,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, intensity, intensity)
- (float, normal_x, normal_x)
- (float, normal_y, normal_y)
- (float, normal_z, normal_z)
- (float, curvature, curvature)
-)
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZLNormal,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (std::uint32_t, label, label)
- (float, normal_x, normal_x)
- (float, normal_y, normal_y)
- (float, normal_z, normal_z)
- (float, curvature, curvature)
-)
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithRange,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, range, range)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointWithViewpoint,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, vp_x, vp_x)
- (float, vp_y, vp_y)
- (float, vp_z, vp_z)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointWithViewpoint, pcl::_PointWithViewpoint)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::MomentInvariants,
- (float, j1, j1)
- (float, j2, j2)
- (float, j3, j3)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PrincipalRadiiRSD,
- (float, r_min, r_min)
- (float, r_max, r_max)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Boundary,
- (std::uint8_t, boundary_point, boundary_point)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PrincipalCurvatures,
- (float, principal_curvature_x, principal_curvature_x)
- (float, principal_curvature_y, principal_curvature_y)
- (float, principal_curvature_z, principal_curvature_z)
- (float, pc1, pc1)
- (float, pc2, pc2)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PFHSignature125,
- (float[125], histogram, pfh)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PFHRGBSignature250,
- (float[250], histogram, pfhrgb)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFSignature,
- (float, f1, f1)
- (float, f2, f2)
- (float, f3, f3)
- (float, f4, f4)
- (float, alpha_m, alpha_m)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature,
- (float, f1, f1)
- (float, f2, f2)
- (float, f3, f3)
- (float, f4, f4)
- (float, f5, f5)
- (float, f6, f6)
- (float, f7, f7)
- (float, f8, f8)
- (float, f9, f9)
- (float, f10, f10)
- (float, alpha_m, alpha_m)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFRGBSignature,
- (float, f1, f1)
- (float, f2, f2)
- (float, f3, f3)
- (float, f4, f4)
- (float, r_ratio, r_ratio)
- (float, g_ratio, g_ratio)
- (float, b_ratio, b_ratio)
- (float, alpha_m, alpha_m)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::NormalBasedSignature12,
- (float[12], values, values)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ShapeContext1980,
- (float[1980], descriptor, shape_context)
- (float[9], rf, rf)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::UniqueShapeContext1960,
- (float[1960], descriptor, shape_context)
- (float[9], rf, rf)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::SHOT352,
- (float[352], descriptor, shot)
- (float[9], rf, rf)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::SHOT1344,
- (float[1344], descriptor, shot)
- (float[9], rf, rf)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::FPFHSignature33,
- (float[33], histogram, fpfh)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::BRISKSignature512,
- (float, scale, brisk_scale)
- (float, orientation, brisk_orientation)
- (unsigned char[64], descriptor, brisk_descriptor512)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::VFHSignature308,
- (float[308], histogram, vfh)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::GRSDSignature21,
- (float[21], histogram, grsd)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ESFSignature640,
- (float[640], histogram, esf)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature512,
- (float[512], histogram, gasd)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature984,
- (float[984], histogram, gasd)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature7992,
- (float[7992], histogram, gasd)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Narf36,
- (float[36], descriptor, descriptor)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::GFPFHSignature16,
- (float[16], histogram, gfpfh)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::IntensityGradient,
- (float, gradient_x, gradient_x)
- (float, gradient_y, gradient_y)
- (float, gradient_z, gradient_z)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithScale,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, scale, scale)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointSurfel,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, normal_x, normal_x)
- (float, normal_y, normal_y)
- (float, normal_z, normal_z)
- (std::uint32_t, rgba, rgba)
- (float, radius, radius)
- (float, confidence, confidence)
- (float, curvature, curvature)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_ReferenceFrame,
- (float[3], x_axis, x_axis)
- (float[3], y_axis, y_axis)
- (float[3], z_axis, z_axis)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::ReferenceFrame, pcl::_ReferenceFrame)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointDEM,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, intensity, intensity)
- (float, intensity_variance, intensity_variance)
- (float, height_variance, height_variance)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointDEM, pcl::_PointDEM)
-
-namespace pcl
-{
- // Allow float 'rgb' data to match to the newer uint32 'rgba' tag. This is so
- // you can load old 'rgb' PCD files into e.g. a PointCloud<PointXYZRGBA>.
- template<typename PointT>
- struct FieldMatches<PointT, fields::rgba>
- {
- bool operator() (const pcl::PCLPointField& field)
- {
- if (field.name == "rgb")
- {
- // For fixing the alpha value bug #1141, the rgb field can also match
- // uint32.
- return ((field.datatype == pcl::PCLPointField::FLOAT32 ||
- field.datatype == pcl::PCLPointField::UINT32) &&
- field.count == 1);
- }
- else
- {
- return (field.name == traits::name<PointT, fields::rgba>::value &&
- field.datatype == traits::datatype<PointT, fields::rgba>::value &&
- field.count == traits::datatype<PointT, fields::rgba>::size);
- }
- }
- };
- template<typename PointT>
- struct FieldMatches<PointT, fields::rgb>
- {
- bool operator() (const pcl::PCLPointField& field)
- {
- if (field.name == "rgba")
- {
- return (field.datatype == pcl::PCLPointField::UINT32 &&
- field.count == 1);
- }
- else
- {
- // For fixing the alpha value bug #1141, rgb can also match uint32
- return (field.name == traits::name<PointT, fields::rgb>::value &&
- (field.datatype == traits::datatype<PointT, fields::rgb>::value ||
- field.datatype == pcl::PCLPointField::UINT32) &&
- field.count == traits::datatype<PointT, fields::rgb>::size);
- }
- }
- };
-
- namespace traits
- {
-
- /** \brief Metafunction to check if a given point type has a given field.
- *
- * Example usage at run-time:
- *
- * \code
- * bool curvature_available = pcl::traits::has_field<PointT, pcl::fields::curvature>::value;
- * \endcode
- *
- * Example usage at compile-time:
- *
- * \code
- * BOOST_MPL_ASSERT_MSG ((pcl::traits::has_field<PointT, pcl::fields::label>::value),
- * POINT_TYPE_SHOULD_HAVE_LABEL_FIELD,
- * (PointT));
- * \endcode
- */
- template <typename PointT, typename Field>
- struct has_field : boost::mpl::contains<typename pcl::traits::fieldList<PointT>::type, Field>::type
- { };
-
- /** Metafunction to check if a given point type has all given fields. */
- template <typename PointT, typename Field>
- struct has_all_fields : boost::mpl::fold<Field,
- boost::mpl::bool_<true>,
- boost::mpl::and_<boost::mpl::_1,
- has_field<PointT, boost::mpl::_2> > >::type
- { };
-
- /** Metafunction to check if a given point type has any of the given fields. */
- template <typename PointT, typename Field>
- struct has_any_field : boost::mpl::fold<Field,
- boost::mpl::bool_<false>,
- boost::mpl::or_<boost::mpl::_1,
- has_field<PointT, boost::mpl::_2> > >::type
- { };
-
- /** \brief Traits defined for ease of use with fields already registered before
- *
- * has_<fields to be detected>: struct with `value` datamember defined at compiletime
- * has_<fields to be detected>_v: constexpr boolean
- * Has<Fields to be detected>: concept modelling name alias for `enable_if`
- */
-
- /** Metafunction to check if a given point type has x and y fields. */
- template <typename PointT>
- struct has_xy : has_all_fields<PointT, boost::mpl::vector<pcl::fields::x,
- pcl::fields::y> >
- { };
-
- template <typename PointT>
- constexpr auto has_xy_v = has_xy<PointT>::value;
-
- template <typename PointT>
- using HasXY = std::enable_if_t<has_xy_v<PointT>, bool>;
-
- template <typename PointT>
- using HasNoXY = std::enable_if_t<!has_xy_v<PointT>, bool>;
-
- /** Metafunction to check if a given point type has x, y, and z fields. */
- template <typename PointT>
- struct has_xyz : has_all_fields<PointT, boost::mpl::vector<pcl::fields::x,
- pcl::fields::y,
- pcl::fields::z> >
- { };
-
- template <typename PointT>
- constexpr auto has_xyz_v = has_xyz<PointT>::value;
-
- template <typename PointT>
- using HasXYZ = std::enable_if_t<has_xyz_v<PointT>, bool>;
-
- template <typename PointT>
- using HasNoXYZ = std::enable_if_t<!has_xyz_v<PointT>, bool>;
-
- /** Metafunction to check if a given point type has normal_x, normal_y, and
- * normal_z fields. */
- template <typename PointT>
- struct has_normal : has_all_fields<PointT, boost::mpl::vector<pcl::fields::normal_x,
- pcl::fields::normal_y,
- pcl::fields::normal_z> >
- { };
-
- template <typename PointT>
- constexpr auto has_normal_v = has_normal<PointT>::value;
-
- template <typename PointT>
- using HasNormal = std::enable_if_t<has_normal_v<PointT>, bool>;
-
- template <typename PointT>
- using HasNoNormal = std::enable_if_t<!has_normal_v<PointT>, bool>;
-
- /** Metafunction to check if a given point type has curvature field. */
- template <typename PointT>
- struct has_curvature : has_field<PointT, pcl::fields::curvature>
- { };
-
- template <typename PointT>
- constexpr auto has_curvature_v = has_curvature<PointT>::value;
-
- template <typename PointT>
- using HasCurvature = std::enable_if_t<has_curvature_v<PointT>, bool>;
-
- template <typename PointT>
- using HasNoCurvature = std::enable_if_t<!has_curvature_v<PointT>, bool>;
-
- /** Metafunction to check if a given point type has intensity field. */
- template <typename PointT>
- struct has_intensity : has_field<PointT, pcl::fields::intensity>
- { };
-
- template <typename PointT>
- constexpr auto has_intensity_v = has_intensity<PointT>::value;
-
- template <typename PointT>
- using HasIntensity = std::enable_if_t<has_intensity_v<PointT>, bool>;
-
- template <typename PointT>
- using HasNoIntensity = std::enable_if_t<!has_intensity_v<PointT>, bool>;
-
- /** Metafunction to check if a given point type has either rgb or rgba field. */
- template <typename PointT>
- struct has_color : has_any_field<PointT, boost::mpl::vector<pcl::fields::rgb,
- pcl::fields::rgba> >
- { };
-
- template <typename PointT>
- constexpr auto has_color_v = has_color<PointT>::value;
-
- template <typename PointT>
- using HasColor = std::enable_if_t<has_color_v<PointT>, bool>;
-
- template <typename PointT>
- using HasNoColor = std::enable_if_t<!has_color_v<PointT>, bool>;
-
- /** Metafunction to check if a given point type has label field. */
- template <typename PointT>
- struct has_label : has_field<PointT, pcl::fields::label>
- { };
-
- template <typename PointT>
- constexpr auto has_label_v = has_label<PointT>::value;
-
- template <typename PointT>
- using HasLabel = std::enable_if_t<has_label_v<PointT>, bool>;
-
- template <typename PointT>
- using HasNoLabel = std::enable_if_t<!has_label_v<PointT>, bool>;
- }
-
} // namespace pcl
+/** @} */
-// Not strictly required, merely to preserve API for PCL users < 1.4
-#include <pcl/common/point_tests.h>
-
-#if defined _MSC_VER
- #pragma warning(default: 4201)
-#endif
-//#pragma warning(pop)
+#include <pcl/impl/point_types.hpp>
/**
* \file bearing_angle_image.h
- * \Created on: July 07, 2012
+ * Created on: July 07, 2012
*/
#pragma once
*
*/
-#ifndef PCL_RANGE_IMAGE_IMPL_HPP_
-#define PCL_RANGE_IMAGE_IMPL_HPP_
+#pragma once
+
+#include <pcl/range_image/range_image.h>
#include <pcl/pcl_macros.h>
#include <pcl/common/distances.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
namespace pcl
{
}
}
-} // namespace end
-#endif
-
+} // namespace pcl
*
*/
-#ifndef PCL_RANGE_IMAGE_PLANAR_IMPL_HPP_
-#define PCL_RANGE_IMAGE_PLANAR_IMPL_HPP_
+#pragma once
-#include <pcl/pcl_macros.h>
#include <pcl/common/eigen.h>
+#include <pcl/range_image/range_image_planar.h>
+#include <pcl/pcl_macros.h>
namespace pcl
{
image_y = center_y_ + focal_length_y_*transformedPoint[1]/transformedPoint[2] - static_cast<float> (image_offset_y_);
}
-} // namespace end
-
-#endif
+} // namespace pcl
*
*/
-#include <pcl/pcl_macros.h>
+#pragma once
+
#include <pcl/common/eigen.h>
+#include <pcl/range_image/range_image_spherical.h>
+#include <pcl/pcl_macros.h>
+
namespace pcl
{
image_x = (angle_x + static_cast<float> (M_PI))*angular_resolution_x_reciprocal_ - static_cast<float> (image_offset_x_);
image_y = (angle_y + 0.5f*static_cast<float> (M_PI))*angular_resolution_y_reciprocal_ - static_cast<float> (image_offset_y_);
}
-} // namespace end
+} // namespace pcl
+
#pragma once
+#include <pcl/memory.h>
#include <pcl/point_cloud.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
namespace pcl
{
/** \brief RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where
- * a 3D scene was captured from a specific view point.
+ * a 3D scene was captured from a specific view point.
* \author Bastian Steder
* \ingroup range_image
*/
using VectorOfEigenVector3f = std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >;
using Ptr = shared_ptr<RangeImage>;
using ConstPtr = shared_ptr<const RangeImage>;
-
+
enum CoordinateFrame
{
CAMERA_FRAME = 0,
LASER_FRAME = 1
};
-
+
// =====CONSTRUCTOR & DESTRUCTOR=====
/** Constructor */
PCL_EXPORTS RangeImage ();
/** Destructor */
PCL_EXPORTS virtual ~RangeImage () = default;
-
+
// =====STATIC VARIABLES=====
/** The maximum number of openmp threads that can be used in this class */
static int max_no_of_threads;
-
+
// =====STATIC METHODS=====
/** \brief Get the size of a certain area when seen from the given pose
* \param viewer_pose an affine matrix defining the pose of the viewer
* \return the size of the area as viewed according to \a viewer_pose
*/
static inline float
- getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center,
+ getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center,
float radius);
-
+
/** \brief Get Eigen::Vector3f from PointWithRange
* \param point the input point
* \return an Eigen::Vector3f representation of the input point
*/
static inline Eigen::Vector3f
getEigenVector3f (const PointWithRange& point);
-
+
/** \brief Get the transformation that transforms the given coordinate frame into CAMERA_FRAME
* \param coordinate_frame the input coordinate frame
* \param transformation the resulting transformation that warps \a coordinate_frame into CAMERA_FRAME
PCL_EXPORTS static void
getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame,
Eigen::Affine3f& transformation);
-
- /** \brief Get the average viewpoint of a point cloud where each point carries viewpoint information as
+
+ /** \brief Get the average viewpoint of a point cloud where each point carries viewpoint information as
* vp_x, vp_y, vp_z
* \param point_cloud the input point cloud
* \return the average viewpoint (as an Eigen::Vector3f)
*/
template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f
getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud);
-
+
/** \brief Check if the provided data includes far ranges and add them to far_ranges
* \param point_cloud_data a PCLPointCloud2 message containing the input cloud
* \param far_ranges the resulting cloud containing those points with far ranges
*/
PCL_EXPORTS static void
extractFarRanges (const pcl::PCLPointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges);
-
+
// =====METHODS=====
/** \brief Get a boost shared pointer of a copy of this */
- inline Ptr
- makeShared () { return Ptr (new RangeImage (*this)); }
-
+ inline Ptr
+ makeShared () { return Ptr (new RangeImage (*this)); }
+
/** \brief Reset all values to an empty range image */
- PCL_EXPORTS void
+ PCL_EXPORTS void
reset ();
-
+
/** \brief Create the depth image from a point cloud
* \param point_cloud the input point cloud
* \param angular_resolution the angular difference (in radians) between the individual pixels in the image
const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
float min_range=0.0f, int border_size=0);
-
+
/** \brief Create the depth image from a point cloud
* \param point_cloud the input point cloud
* \param angular_resolution_x the angular difference (in radians) between the
const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
CoordinateFrame coordinate_frame=CAMERA_FRAME,
float noise_level=0.0f, float min_range=0.0f, int border_size=0);
-
- /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for
+
+ /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for
* faster calculation.
* \param point_cloud the input point cloud
* \param angular_resolution the angle (in radians) between each sample in the depth image
const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
CoordinateFrame coordinate_frame=CAMERA_FRAME,
float noise_level=0.0f, float min_range=0.0f, int border_size=0);
-
- /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for
+
+ /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for
* faster calculation.
* \param point_cloud the input point cloud
* \param angular_resolution_x the angular difference (in radians) between the
const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
CoordinateFrame coordinate_frame=CAMERA_FRAME,
float noise_level=0.0f, float min_range=0.0f, int border_size=0);
-
- /** \brief Create the depth image from a point cloud, using the average viewpoint of the points
+
+ /** \brief Create the depth image from a point cloud, using the average viewpoint of the points
* (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)).
* \param point_cloud the input point cloud
* \param angular_resolution the angle (in radians) between each sample in the depth image
* \param min_range the minimum visible range (defaults to 0)
* \param border_size the border size (defaults to 0)
* \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame
- * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y
+ * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y
* to the bottom and z to the front) */
template <typename PointCloudTypeWithViewpoints> void
createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution,
float max_angle_width, float max_angle_height,
CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
float min_range=0.0f, int border_size=0);
-
- /** \brief Create the depth image from a point cloud, using the average viewpoint of the points
+
+ /** \brief Create the depth image from a point cloud, using the average viewpoint of the points
* (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)).
* \param point_cloud the input point cloud
* \param angular_resolution_x the angular difference (in radians) between the
* \param min_range the minimum visible range (defaults to 0)
* \param border_size the border size (defaults to 0)
* \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame
- * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y
+ * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y
* to the bottom and z to the front) */
template <typename PointCloudTypeWithViewpoints> void
createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
float max_angle_width, float max_angle_height,
CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
float min_range=0.0f, int border_size=0);
-
+
/** \brief Create an empty depth image (filled with unobserved points)
* \param[in] angular_resolution the angle (in radians) between each sample in the depth image
* \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () )
void
createEmpty (float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
- float angle_height=pcl::deg2rad (180.0f));
-
+ float angle_height=pcl::deg2rad (180.0f));
+
/** \brief Create an empty depth image (filled with unobserved points)
* \param angular_resolution_x the angular difference (in radians) between the
* individual pixels in the image in the x-direction
const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
float angle_height=pcl::deg2rad (180.0f));
-
+
/** \brief Integrate the given point cloud into the current range image using a z-buffer
* \param point_cloud the input point cloud
* \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
* \param top returns the minimum y pixel position in the image where a point was added
* \param right returns the maximum x pixel position in the image where a point was added
* \param bottom returns the maximum y pixel position in the image where a point was added
- * \param top returns the minimum y position in the image where a point was added
* \param left returns the minimum x pixel position in the image where a point was added
*/
template <typename PointCloudType> void
doZBuffer (const PointCloudType& point_cloud, float noise_level,
float min_range, int& top, int& right, int& bottom, int& left);
-
+
/** \brief Integrates the given far range measurements into the range image */
template <typename PointCloudType> void
integrateFarRanges (const PointCloudType& far_ranges);
-
+
/** \brief Cut the range image to the minimal size so that it still contains all actual range readings.
* \param border_size allows increase from the minimal size by the specified number of pixels (defaults to 0)
* \param top if positive, this value overrides the position of the top edge (defaults to -1)
*/
PCL_EXPORTS void
cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1);
-
- /** \brief Get all the range values in one float array of size width*height
+
+ /** \brief Get all the range values in one float array of size width*height
* \return a pointer to a new float array containing the range values
* \note This method allocates a new float array; the caller is responsible for freeing this memory.
*/
PCL_EXPORTS float*
getRangesArray () const;
-
+
/** Getter for the transformation from the world system into the range image system
* (the sensor coordinate frame) */
inline const Eigen::Affine3f&
getTransformationToRangeImageSystem () const { return (to_range_image_system_); }
-
+
/** Setter for the transformation from the range image system
* (the sensor coordinate frame) into the world system */
- inline void
+ inline void
setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system);
-
+
/** Getter for the transformation from the range image system
* (the sensor coordinate frame) into the world system */
inline const Eigen::Affine3f&
getTransformationToWorldSystem () const { return to_world_system_;}
-
+
/** Getter for the angular resolution of the range image in x direction in radians per pixel.
* Provided for downwards compatibility */
inline float
getAngularResolution () const { return angular_resolution_x_;}
-
+
/** Getter for the angular resolution of the range image in x direction in radians per pixel. */
inline float
getAngularResolutionX () const { return angular_resolution_x_;}
-
+
/** Getter for the angular resolution of the range image in y direction in radians per pixel. */
inline float
getAngularResolutionY () const { return angular_resolution_y_;}
-
+
/** Getter for the angular resolution of the range image in x and y direction (in radians). */
inline void
getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const;
-
+
/** \brief Set the angular resolution of the range image
* \param angular_resolution the new angular resolution in x and y direction (in radians per pixel)
*/
inline void
setAngularResolution (float angular_resolution);
-
+
/** \brief Set the angular resolution of the range image
* \param angular_resolution_x the new angular resolution in x direction (in radians per pixel)
* \param angular_resolution_y the new angular resolution in y direction (in radians per pixel)
inline void
setAngularResolution (float angular_resolution_x, float angular_resolution_y);
-
+
/** \brief Return the 3D point with range at the given image position
* \param image_x the x coordinate
* \param image_y the y coordinate
/** \brief Non-const-version of getPoint */
inline PointWithRange&
getPoint (int image_x, int image_y);
-
+
/** Return the 3d point with range at the given image position */
inline const PointWithRange&
getPoint (float image_x, float image_y) const;
-
+
/** Non-const-version of the above */
inline PointWithRange&
getPoint (float image_x, float image_y);
-
+
/** \brief Return the 3D point with range at the given image position. This methd performs no error checking
* to make sure the specified image position is inside of the image!
* \param image_x the x coordinate
/** Same as above */
inline void
getPoint (int image_x, int image_y, Eigen::Vector3f& point) const;
-
+
/** Same as above */
inline void
getPoint (int index, Eigen::Vector3f& point) const;
-
+
/** Same as above */
inline const Eigen::Map<const Eigen::Vector3f>
getEigenVector3f (int x, int y) const;
/** Same as above */
inline const Eigen::Map<const Eigen::Vector3f>
getEigenVector3f (int index) const;
-
+
/** Return the 3d point with range at the given index (whereas index=y*width+x) */
inline const PointWithRange&
getPoint (int index) const;
/** Calculate the 3D point according to the given image point and the range value at the closest pixel */
inline void
calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const;
-
+
/** Recalculate all 3D point positions according to their pixel position and range */
PCL_EXPORTS void
recalculate3DPointPositions ();
-
+
/** Get imagePoint from 3D point in world coordinates */
inline virtual void
getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;
/** Same as above */
inline void
getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const;
-
+
/** Same as above */
inline void
getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const;
-
+
/** Same as above */
inline void
getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const;
-
+
/** Same as above */
inline void
getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const;
-
+
/** Same as above */
inline void
getImagePoint (float x, float y, float z, float& image_x, float& image_y) const;
-
+
/** Same as above */
inline void
getImagePoint (float x, float y, float z, int& image_x, int& image_y) const;
-
+
/** point_in_image will be the point in the image at the position the given point would be. Returns
* the range of the given point. */
inline float
* (Return value is point_in_image.range-given_point.range) */
inline float
getRangeDifference (const Eigen::Vector3f& point) const;
-
+
/** Get the image point corresponding to the given angles */
inline void
getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const;
-
+
/** Get the angles corresponding to the given image point */
inline void
getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const;
-
+
/** Transforms an image point in float values to an image point in int values */
inline void
real2DToInt2D (float x, float y, int& xInt, int& yInt) const;
-
+
/** Check if a point is inside of the image */
inline bool
isInImage (int x, int y) const;
-
+
/** Check if a point is inside of the image and has a finite range */
inline bool
isValid (int x, int y) const;
-
+
/** Check if a point has a finite range */
inline bool
isValid (int index) const;
-
+
/** Check if a point is inside of the image and has either a finite range or a max reading (range=INFINITY) */
inline bool
isObserved (int x, int y) const;
/** Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! */
inline bool
isMaxRange (int x, int y) const;
-
+
/** Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius.
* step_size determines how many pixels are used. 1 means all, 2 only every second, etc..
* Returns false if it was unable to calculate a normal.*/
inline bool
getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const;
-
+
/** Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered. */
inline bool
getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const;
-
+
/** Same as above */
inline bool
getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point,
int no_of_nearest_neighbors, Eigen::Vector3f& normal,
Eigen::Vector3f* point_on_plane=nullptr, int step_size=1) const;
-
+
/** Same as above, using default values */
inline bool
getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const;
-
+
/** Same as above but extracts some more data and can also return the extracted
* information for all neighbors in radius if normal_all_neighbors is not NULL */
inline bool
Eigen::Vector3f* normal_all_neighbors=nullptr,
Eigen::Vector3f* mean_all_neighbors=nullptr,
Eigen::Vector3f* eigen_values_all_neighbors=nullptr) const;
-
+
// Return the squared distance to the n-th neighbors of the point at x,y
inline float
getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const;
-
+
/** Calculate the impact angle based on the sensor position and the two given points - will return
* -INFINITY if one of the points is unobserved */
inline float
//! Same as above
inline float
getImpactAngle (int x1, int y1, int x2, int y2) const;
-
+
/** Extract a local normal (with a heuristic not to include background points) and calculate the impact
* angle based on this */
inline float
* Will return -INFINITY if no normal could be calculated */
inline float
getNormalBasedAcutenessValue (int x, int y, int radius) const;
-
+
/** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg)
* will return -INFINITY if one of the points is unobserved */
inline float
//! Same as above
inline float
getAcutenessValue (int x1, int y1, int x2, int y2) const;
-
+
/** Calculate getAcutenessValue for every point */
PCL_EXPORTS void
getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x,
float*& acuteness_value_image_y) const;
-
+
/** Calculates, how much the surface changes at a point. Pi meaning a flat suface and 0.0f
* would be a needle point */
//inline float
// getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1,
// const PointWithRange& neighbor2) const;
-
+
/** Calculates, how much the surface changes at a point. 1 meaning a 90deg angle and 0 a flat suface */
PCL_EXPORTS float
getSurfaceChange (int x, int y, int radius) const;
-
+
/** Uses the above function for every point in the image */
PCL_EXPORTS float*
getSurfaceChangeImage (int radius) const;
-
+
/** Calculates, how much the surface changes at a point. Returns an angle [0.0f, PI] for x and y direction.
* A return value of -INFINITY means that a point was unobserved. */
inline void
getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const;
-
+
/** Uses the above function for every point in the image */
PCL_EXPORTS void
getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const;
-
+
/** Calculates the curvature in a point using pca */
inline float
getCurvature (int x, int y, int radius, int step_size) const;
-
+
//! Get the sensor position
inline const Eigen::Vector3f
getSensorPos () const;
-
+
/** Sets all -INFINITY values to INFINITY */
PCL_EXPORTS void
setUnseenToMaxRange ();
-
+
//! Getter for image_offset_x_
inline int
getImageOffsetX () const { return image_offset_x_;}
//! Getter for image_offset_y_
inline int
getImageOffsetY () const { return image_offset_y_;}
-
+
//! Setter for image offsets
inline void
setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;}
-
-
+
+
/** Get a sub part of the complete image as a new range image.
* \param sub_image_image_offset_x - The x coordinate of the top left pixel of the sub image.
* This is always according to absolute 0,0 meaning -180°,-90°
virtual void
getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
int sub_image_height, int combine_pixels, RangeImage& sub_image) const;
-
+
//! Get a range image with half the resolution
- virtual void
+ virtual void
getHalfImage (RangeImage& half_image) const;
-
+
//! Find the minimum and maximum range in the image
PCL_EXPORTS void
getMinMaxRanges (float& min_range, float& max_range) const;
-
+
//! This function sets the sensor pose to 0 and transforms all point positions to this local coordinate frame
PCL_EXPORTS void
change3dPointsToLocalCoordinateFrame ();
-
+
/** Calculate a range patch as the z values of the coordinate frame given by pose.
* The patch will have size pixel_size x pixel_size and each pixel
* covers world_size/pixel_size meters in the world
* You are responsible for deleting the structure afterwards! */
PCL_EXPORTS float*
getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const;
-
+
//! Same as above, but using the local coordinate frame defined by point and the viewing direction
PCL_EXPORTS float*
getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const;
-
+
//! Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction
inline Eigen::Affine3f
getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const;
PCL_EXPORTS bool
getNormalBasedUprightTransformation (const Eigen::Vector3f& point,
float max_dist, Eigen::Affine3f& transformation) const;
-
+
/** Get the integral image of the range values (used for fast blur operations).
* You are responsible for deleting it after usage! */
PCL_EXPORTS void
getIntegralImage (float*& integral_image, int*& valid_points_num_image) const;
-
+
/** Get a blurred version of the range image using box filters on the provided integral image*/
PCL_EXPORTS void // Template necessary so that this function also works in derived classes
getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image,
RangeImage& range_image) const;
-
+
/** Get a blurred version of the range image using box filters */
PCL_EXPORTS virtual void // Template necessary so that this function also works in derived classes
getBlurredImage (int blur_radius, RangeImage& range_image) const;
-
+
/** Get the squared euclidean distance between the two image points.
* Returns -INFINITY if one of the points was not observed */
inline float
//! Doing the above for some steps in the given direction and averaging
inline float
getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const;
-
+
//! Project all points on the local plane approximation, thereby smoothing the surface of the scan
PCL_EXPORTS void
getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const;
//void getLocalNormals (int radius) const;
-
+
/** Calculates the average 3D position of the no_of_points points described by the start
* point x,y in the direction delta.
* Returns a max range point (range=INFINITY) if the first point is max range and an
inline void
get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points,
PointWithRange& average_point) const;
-
+
/** Calculates the overlap of two range images given the relative transformation
* (from the given image to *this) */
PCL_EXPORTS float
getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation,
int search_radius, float max_distance, int pixel_step=1) const;
-
+
/** Get the viewing direction for the given point */
inline bool
getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const;
-
+
/** Get the viewing direction for the given point */
inline void
getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const;
-
+
/** Return a newly created Range image.
* Can be reimplemented in derived classes like RangeImagePlanar to return an image of the same type. */
- PCL_EXPORTS virtual RangeImage*
+ PCL_EXPORTS virtual RangeImage*
getNew () const { return new RangeImage; }
/** Copy other to *this. Necessary for use in virtual functions that need to copy derived RangeImage classes (like RangeImagePlanar) */
PCL_EXPORTS virtual void
copyTo (RangeImage& other) const;
-
+
// =====MEMBER VARIABLES=====
// BaseClass:
// roslib::Header header;
// bool is_dense;
static bool debug; /**< Just for... well... debugging purposes. :-) */
-
+
protected:
// =====PROTECTED MEMBER VARIABLES=====
Eigen::Affine3f to_range_image_system_; /**< Inverse of to_world_system_ */
* an image of full size (360x180 degrees) */
PointWithRange unobserved_point; /**< This point is used to be able to return
* a reference to a non-existing point */
-
+
// =====PROTECTED METHODS=====
/** Query the asin lookup table */
static inline float
asinLookUp (float value);
-
+
/** Query the std::atan2 lookup table */
static inline float
atan2LookUp (float y, float x);
-
+
/** Query the cos lookup table */
static inline float
cosLookUp (float value);
//https://bugreports.qt-project.org/browse/QTBUG-22829
#ifndef Q_MOC_RUN
#include <pcl/pcl_macros.h>
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
#include <boost/mpl/vector.hpp>
#include <boost/preprocessor/seq/enum.hpp>
#include <boost/preprocessor/seq/for_each.hpp>
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2020-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include <boost/mpl/assert.hpp>
+
+// This is required for the workaround at line 109
+#ifdef _MSC_VER
+#include <Eigen/Core>
+#include <Eigen/src/StlSupport/details.h>
+#endif
+
+#include <string>
+#include <type_traits>
+
+#include <cstdint>
+
+namespace pcl
+{
+ namespace deprecated
+ {
+ /** \class DeprecatedType
+ * \brief A dummy type to aid in template parameter deprecation
+ */
+ struct T {};
+ }
+
+ namespace fields
+ {
+ // Tag types get put in this namespace
+ }
+
+ namespace traits
+ {
+ namespace detail {
+ /**
+ * \brief Enumeration for different numerical types
+ *
+ * \details struct used to enable scope and implicit conversion to int
+ */
+ struct PointFieldTypes {
+ static const std::uint8_t INT8 = 1, UINT8 = 2,
+ INT16 = 3, UINT16 = 4,
+ INT32 = 5, UINT32 = 6,
+ FLOAT32 = 7, FLOAT64 = 8;
+ };
+ }
+
+ // Metafunction to return enum value representing a type
+ template<typename T> struct asEnum {};
+ template<> struct asEnum<std::int8_t> { static const std::uint8_t value = detail::PointFieldTypes::INT8; };
+ template<> struct asEnum<std::uint8_t> { static const std::uint8_t value = detail::PointFieldTypes::UINT8; };
+ template<> struct asEnum<std::int16_t> { static const std::uint8_t value = detail::PointFieldTypes::INT16; };
+ template<> struct asEnum<std::uint16_t> { static const std::uint8_t value = detail::PointFieldTypes::UINT16; };
+ template<> struct asEnum<std::int32_t> { static const std::uint8_t value = detail::PointFieldTypes::INT32; };
+ template<> struct asEnum<std::uint32_t> { static const std::uint8_t value = detail::PointFieldTypes::UINT32; };
+ template<> struct asEnum<float> { static const std::uint8_t value = detail::PointFieldTypes::FLOAT32; };
+ template<> struct asEnum<double> { static const std::uint8_t value = detail::PointFieldTypes::FLOAT64; };
+
+ template<typename T>
+ static constexpr std::uint8_t asEnum_v = asEnum<T>::value;
+
+ // Metafunction to return type of enum value
+ template<int> struct asType {};
+ template<> struct asType<detail::PointFieldTypes::INT8> { using type = std::int8_t; };
+ template<> struct asType<detail::PointFieldTypes::UINT8> { using type = std::uint8_t; };
+ template<> struct asType<detail::PointFieldTypes::INT16> { using type = std::int16_t; };
+ template<> struct asType<detail::PointFieldTypes::UINT16> { using type = std::uint16_t; };
+ template<> struct asType<detail::PointFieldTypes::INT32> { using type = std::int32_t; };
+ template<> struct asType<detail::PointFieldTypes::UINT32> { using type = std::uint32_t; };
+ template<> struct asType<detail::PointFieldTypes::FLOAT32> { using type = float; };
+ template<> struct asType<detail::PointFieldTypes::FLOAT64> { using type = double; };
+
+ template<int index>
+ using asType_t = typename asType<index>::type;
+
+ // Metafunction to decompose a type (possibly of array of any number of dimensions) into
+ // its scalar type and total number of elements.
+ template<typename T> struct decomposeArray
+ {
+ using type = std::remove_all_extents_t<T>;
+ static const std::uint32_t value = sizeof (T) / sizeof (type);
+ };
+
+ // For non-POD point types, this is specialized to return the corresponding POD type.
+ template<typename PointT>
+ struct POD
+ {
+ using type = PointT;
+ };
+
+#ifdef _MSC_VER
+
+ /* Sometimes when calling functions like `copyPoint()` or `copyPointCloud`
+ * without explicitly specifying point types, MSVC deduces them to be e.g.
+ * `Eigen::internal::workaround_msvc_stl_support<pcl::PointXYZ>` instead of
+ * plain `pcl::PointXYZ`. Subsequently these types are passed to meta-
+ * functions like `has_field` or `fieldList` and make them choke. This hack
+ * makes use of the fact that internally `fieldList` always applies `POD` to
+ * its argument type. This specialization therefore allows to unwrap the
+ * contained point type. */
+ template<typename PointT>
+ struct POD<Eigen::internal::workaround_msvc_stl_support<PointT> >
+ {
+ using type = PointT;
+ };
+
+#endif
+
+ // name
+ /* This really only depends on Tag, but we go through some gymnastics to avoid ODR violations.
+ We template it on the point type PointT to avoid ODR violations when registering multiple
+ point types with shared tags.
+ The dummy parameter is so we can partially specialize name on PointT and Tag but leave it
+ templated on dummy. Each specialization declares a static char array containing the tag
+ name. The definition of the static member would conflict when linking multiple translation
+ units that include the point type registration. But when the static member definition is
+ templated (on dummy), we sidestep the ODR issue.
+ */
+ template<class PointT, typename Tag, int dummy = 0>
+ struct name : name<typename POD<PointT>::type, Tag, dummy>
+ {
+ // Contents of specialization:
+ // static const char value[];
+
+ // Avoid infinite compile-time recursion
+ BOOST_MPL_ASSERT_MSG((!std::is_same<PointT, typename POD<PointT>::type>::value),
+ POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
+ };
+
+ // offset
+ template<class PointT, typename Tag>
+ struct offset : offset<typename POD<PointT>::type, Tag>
+ {
+ // Contents of specialization:
+ // static const std::size_t value;
+
+ // Avoid infinite compile-time recursion
+ BOOST_MPL_ASSERT_MSG((!std::is_same<PointT, typename POD<PointT>::type>::value),
+ POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
+ };
+
+ // datatype
+ template<class PointT, typename Tag>
+ struct datatype : datatype<typename POD<PointT>::type, Tag>
+ {
+ // Contents of specialization:
+ // using type = ...;
+ // static const std::uint8_t value;
+ // static const std::uint32_t size;
+
+ // Avoid infinite compile-time recursion
+ BOOST_MPL_ASSERT_MSG((!std::is_same<PointT, typename POD<PointT>::type>::value),
+ POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
+ };
+
+ // fields
+ template<typename PointT>
+ struct fieldList : fieldList<typename POD<PointT>::type>
+ {
+ // Contents of specialization:
+ // using type = boost::mpl::vector<...>;
+
+ // Avoid infinite compile-time recursion
+ BOOST_MPL_ASSERT_MSG((!std::is_same<PointT, typename POD<PointT>::type>::value),
+ POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
+ };
+ } //namespace traits
+
+ /** \brief A helper functor that can copy a specific value if the given field exists.
+ *
+ * \note In order to actually copy the value an instance of this functor should be passed
+ * to a pcl::for_each_type loop. See the example below.
+ *
+ * \code
+ * PointInT p;
+ * bool exists;
+ * float value;
+ * using FieldList = typename pcl::traits::fieldList<PointInT>::type;
+ * pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<PointT, float> (p, "intensity", exists, value));
+ * \endcode
+ */
+ template <typename PointInT, typename OutT>
+ struct CopyIfFieldExists
+ {
+ using Pod = typename traits::POD<PointInT>::type;
+
+ /** \brief Constructor.
+ * \param[in] pt the input point
+ * \param[in] field the name of the field
+ * \param[out] exists set to true if the field exists, false otherwise
+ * \param[out] value the copied field value
+ */
+ CopyIfFieldExists (const PointInT &pt,
+ const std::string &field,
+ bool &exists,
+ OutT &value)
+ : pt_ (reinterpret_cast<const Pod&>(pt)), name_ (field), exists_ (exists), value_ (value)
+ {
+ exists_ = false;
+ }
+
+ /** \brief Constructor.
+ * \param[in] pt the input point
+ * \param[in] field the name of the field
+ * \param[out] value the copied field value
+ */
+ CopyIfFieldExists (const PointInT &pt,
+ const std::string &field,
+ OutT &value)
+ : pt_ (reinterpret_cast<const Pod&>(pt)), name_ (field), exists_ (exists_tmp_), value_ (value)
+ {
+ }
+
+ /** \brief Operator. Data copy happens here. */
+ template <typename Key> inline void
+ operator() ()
+ {
+ if (name_ == pcl::traits::name<PointInT, Key>::value)
+ {
+ exists_ = true;
+ using T = typename pcl::traits::datatype<PointInT, Key>::type;
+ const std::uint8_t* data_ptr = reinterpret_cast<const std::uint8_t*>(&pt_) + pcl::traits::offset<PointInT, Key>::value;
+ value_ = static_cast<OutT> (*reinterpret_cast<const T*>(data_ptr));
+ }
+ }
+
+ private:
+ const Pod &pt_;
+ const std::string &name_;
+ bool &exists_;
+ // Bogus entry
+ bool exists_tmp_;
+ OutT &value_;
+ };
+
+ /** \brief A helper functor that can set a specific value in a field if the field exists.
+ *
+ * \note In order to actually set the value an instance of this functor should be passed
+ * to a pcl::for_each_type loop. See the example below.
+ *
+ * \code
+ * PointT p;
+ * using FieldList = typename pcl::traits::fieldList<PointT>::type;
+ * pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<PointT, float> (p, "intensity", 42.0f));
+ * \endcode
+ */
+ template <typename PointOutT, typename InT>
+ struct SetIfFieldExists
+ {
+ using Pod = typename traits::POD<PointOutT>::type;
+
+ /** \brief Constructor.
+ * \param[in] pt the input point
+ * \param[in] field the name of the field
+ * \param[out] value the value to set
+ */
+ SetIfFieldExists (PointOutT &pt,
+ const std::string &field,
+ const InT &value)
+ : pt_ (reinterpret_cast<Pod&>(pt)), name_ (field), value_ (value)
+ {
+ }
+
+ /** \brief Operator. Data copy happens here. */
+ template <typename Key> inline void
+ operator() ()
+ {
+ if (name_ == pcl::traits::name<PointOutT, Key>::value)
+ {
+ using T = typename pcl::traits::datatype<PointOutT, Key>::type;
+ std::uint8_t* data_ptr = reinterpret_cast<std::uint8_t*>(&pt_) + pcl::traits::offset<PointOutT, Key>::value;
+ *reinterpret_cast<T*>(data_ptr) = static_cast<T> (value_);
+ }
+ }
+
+ private:
+ Pod &pt_;
+ const std::string &name_;
+ const InT &value_;
+ };
+
+ /** \brief Set the value at a specified field in a point
+ * \param[out] pt the point to set the value to
+ * \param[in] field_offset the offset of the field
+ * \param[in] value the value to set
+ */
+ template <typename PointT, typename ValT> inline void
+ setFieldValue (PointT &pt, std::size_t field_offset, const ValT &value)
+ {
+ std::uint8_t* data_ptr = reinterpret_cast<std::uint8_t*>(&pt) + field_offset;
+ *reinterpret_cast<ValT*>(data_ptr) = value;
+ }
+
+ /** \brief Get the value at a specified field in a point
+ * \param[in] pt the point to get the value from
+ * \param[in] field_offset the offset of the field
+ * \param[out] value the value to retrieve
+ */
+ template <typename PointT, typename ValT> inline void
+ getFieldValue (const PointT &pt, std::size_t field_offset, ValT &value)
+ {
+ const std::uint8_t* data_ptr = reinterpret_cast<const std::uint8_t*>(&pt) + field_offset;
+ value = *reinterpret_cast<const ValT*>(data_ptr);
+ }
+
+ template <typename ...> using void_t = void; // part of std in c++17
+
+#ifdef DOXYGEN_ONLY
+
+ /**
+ * \brief Tests at compile time if type T has a custom allocator
+ *
+ * \see pcl::make_shared, PCL_MAKE_ALIGNED_OPERATOR_NEW
+ * \tparam T Type of the object to test
+ */
+ template <typename T> struct has_custom_allocator;
+
+#else
+
+ template <typename, typename = void_t<>> struct has_custom_allocator : std::false_type {};
+ template <typename T> struct has_custom_allocator<T, void_t<typename T::_custom_allocator_type_trait>> : std::true_type {};
+
+#endif
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2020-, OpenPerception
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+
+/**
+ * \file pcl/types.h
+ *
+ * \brief Defines basic non-point types used by PCL
+ * \ingroup common
+ */
+
+#include <pcl/pcl_macros.h>
+
+#include <vector>
+
+#include <cstdint>
+
+namespace pcl
+{
+ using uint8_t PCL_DEPRECATED(1, 12, "use std::uint8_t instead of pcl::uint8_t") = std::uint8_t;
+ using int8_t PCL_DEPRECATED(1, 12, "use std::int8_t instead of pcl::int8_t") = std::int8_t;
+ using uint16_t PCL_DEPRECATED(1, 12, "use std::uint16_t instead of pcl::uint16_t") = std::uint16_t;
+ using int16_t PCL_DEPRECATED(1, 12, "use std::uint16_t instead of pcl::int16_t") = std::int16_t;
+ using uint32_t PCL_DEPRECATED(1, 12, "use std::uint32_t instead of pcl::uint32_t") = std::uint32_t;
+ using int32_t PCL_DEPRECATED(1, 12, "use std::int32_t instead of pcl::int32_t") = std::int32_t;
+ using uint64_t PCL_DEPRECATED(1, 12, "use std::uint64_t instead of pcl::uint64_t") = std::uint64_t;
+ using int64_t PCL_DEPRECATED(1, 12, "use std::int64_t instead of pcl::int64_t") = std::int64_t;
+ using int_fast16_t PCL_DEPRECATED(1, 12, "use std::int_fast16_t instead of pcl::int_fast16_t") = std::int_fast16_t;
+
+// temporary macros for customization. Only use for PCL < 1.12
+// Aim is to remove macros and instead allow multiple index types to coexist together
+#ifndef PCL_INDEX_SIZE
+#if PCL_MINOR_VERSION <= 11
+// sizeof returns bytes, while we measure size by bits in the template
+#define PCL_INDEX_SIZE (sizeof(int) * 8)
+#else
+#define PCL_INDEX_SIZE 32
+#endif // PCL_MINOR_VERSION
+#endif // PCL_INDEX_SIZE
+
+#ifndef PCL_INDEX_SIGNED
+#define PCL_INDEX_SIGNED true
+#endif
+
+ namespace detail {
+ /**
+ * \brief int_type::type refers to an integral type that satisfies template parameters
+ * \tparam Bits number of bits in the integral type
+ * \tparam Signed signed or unsigned nature of the type
+ */
+ template <std::size_t Bits, bool Signed = true>
+ struct int_type { using type = void; };
+
+ /**
+ * \brief helper type to use for `int_type::type`
+ * \see int_type
+ */
+ template <std::size_t Bits, bool Signed = true>
+ using int_type_t = typename int_type<Bits, Signed>::type;
+
+ template <>
+ struct int_type<8, true> { using type = std::int8_t; };
+ template <>
+ struct int_type<8, false> { using type = std::uint8_t; };
+ template <>
+ struct int_type<16, true> { using type = std::int16_t; };
+ template <>
+ struct int_type<16, false> { using type = std::uint16_t; };
+ template <>
+ struct int_type<32, true> { using type = std::int32_t; };
+ template <>
+ struct int_type<32, false> { using type = std::uint32_t; };
+ template <>
+ struct int_type<64, true> { using type = std::int64_t; };
+ template <>
+ struct int_type<64, false> { using type = std::uint64_t; };
+
+ /**
+ * \brief number of bits in PCL's index type
+ *
+ * For PCL 1.11, please use PCL_INDEX_SIZE to choose a size best suited for your needs.
+ * PCL 1.12 will come with default 32, along with client code compile time choice
+ *
+ * PCL 1.11 has a default size = sizeof(int)
+ */
+ constexpr std::uint8_t index_type_size = PCL_INDEX_SIZE;
+
+ /**
+ * \brief signed/unsigned nature of PCL's index type
+ * For PCL 1.11, please use PCL_INDEX_SIGNED to choose a type best suited for your needs.
+ * PCL 1.12 will come with default signed, along with client code compile time choice
+ * Default: signed
+ */
+ constexpr bool index_type_signed = PCL_INDEX_SIGNED;
+} // namespace detail
+
+ /**
+ * \brief Type used for an index in PCL
+ *
+ * Default index_t = int for PCL 1.11, std::int32_t for PCL >= 1.12
+ */
+ using index_t = detail::int_type_t<detail::index_type_size, detail::index_type_signed>;
+ static_assert(!std::is_void<index_t>::value, "`index_t` can't have type `void`");
+
+ /**
+ * \brief Type used for indices in PCL
+ */
+ using Indices = std::vector<index_t>;
+} // namespace pcl
+
int k;
// execute the p different work units in different threads
-# pragma omp parallel for
+// We cannot use OPENMP_LEGACY_CONST_DATA_SHARING_RULE here, because we cannot include
+// pcl_macros.h in this file as this is a C file.
+#if (defined _OPENMP && (_OPENMP <= 201307)) || (defined __GNUC__ && (__GNUC__ >= 6 && __GNUC__ < 9))
+#pragma omp parallel for \
+ default(none) \
+ shared(f, factors, Fout, in_stride)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(f, factors, Fout, fstride, in_stride, m, p, st)
+#endif
for (k=0;k<p;++k)
kf_work( Fout +k*m, f+ fstride*in_stride*k,fstride*p,in_stride,factors,st);
// all threads have joined by this point
assert (kernel.size () % 2 == 1);
std::size_t kernel_width = kernel.size () -1;
std::size_t radius = kernel.size () / 2;
- const pcl::PointCloud<float>* input_;
+ std::unique_ptr<const pcl::PointCloud<float>> copied_input;
+ const pcl::PointCloud<float>* unaliased_input;
if (&input != &output)
{
if (output.height < input.height || output.width < input.width)
output.height = input.height;
output.points.resize (input.height * input.width);
}
- input_ = &input;
+ unaliased_input = &input;
}
else
- input_ = new pcl::PointCloud<float>(input);
+ {
+ copied_input = std::make_unique<pcl::PointCloud<float>>(input);
+ unaliased_input = copied_input.get();
+ }
std::size_t i;
- for (std::size_t j = 0; j < input_->height; j++)
+ for (std::size_t j = 0; j < unaliased_input->height; j++)
{
for (i = 0 ; i < radius ; i++)
output (i,j) = 0;
- for ( ; i < input_->width - radius ; i++)
+ for ( ; i < unaliased_input->width - radius ; i++)
{
output (i,j) = 0;
for (int k = static_cast<int>(kernel_width), l = static_cast<int>(i - radius); k >= 0 ; k--, l++)
- output (i,j) += (*input_) (l,j) * kernel[k];
+ output (i,j) += (*unaliased_input) (l,j) * kernel[k];
}
- for ( ; i < input_->width ; i++)
+ for ( ; i < unaliased_input->width ; i++)
output (i,j) = 0;
}
-
- if (&input == &output)
- {
- delete input_;
- }
}
void
assert (kernel.size () % 2 == 1);
std::size_t kernel_width = kernel.size () -1;
std::size_t radius = kernel.size () / 2;
- const pcl::PointCloud<float>* input_;
+ std::unique_ptr<const pcl::PointCloud<float>> copied_input;
+ const pcl::PointCloud<float>* unaliased_input;
if (&input != &output)
{
if (output.height < input.height || output.width < input.width)
{
output.width = input.width;
output.height = input.height;
- output.resize (input.width * input.height);
+ output.points.resize (input.height * input.width);
}
- input_ = &input;
+ unaliased_input = &input;
}
else
- input_ = new pcl::PointCloud<float> (input);
+ {
+ copied_input = std::make_unique<pcl::PointCloud<float>>(input);
+ unaliased_input = copied_input.get();
+ }
std::size_t j;
- for (std::size_t i = 0; i < input_->width; i++)
+ for (std::size_t i = 0; i < unaliased_input->width; i++)
{
for (j = 0 ; j < radius ; j++)
output (i,j) = 0;
- for ( ; j < input_->height - radius ; j++) {
+ for ( ; j < unaliased_input->height - radius ; j++) {
output (i,j) = 0;
for (int k = static_cast<int>(kernel_width), l = static_cast<int>(j - radius) ; k >= 0 ; k--, l++)
{
- output (i,j) += (*input_) (i,l) * kernel[k];
+ output (i,j) += (*unaliased_input) (i,l) * kernel[k];
}
}
- for ( ; j < input_->height ; j++)
+ for ( ; j < unaliased_input->height ; j++)
output (i,j) = 0;
}
- if (&input == &output)
- delete input_;
}
float max_distance_squared = max_distance*max_distance;
- # pragma omp parallel for num_threads (max_no_of_threads) default (shared) schedule (dynamic, 1) \
- reduction (+ : valid_points_counter) reduction (+ : hits_counter)
+#pragma omp parallel for \
+ default(none) \
+ shared(max_distance_squared, other_range_image, pixel_step, relative_transformation, search_radius) \
+ schedule(dynamic, 1) \
+ reduction(+ : valid_points_counter) \
+ reduction(+ : hits_counter) \
+ num_threads(max_no_of_threads)
for (int other_y=0; other_y<int (other_range_image.height); other_y+=pixel_step)
{
for (int other_x=0; other_x<int (other_range_image.width); other_x+=pixel_step)
*
*/
-#include <pcl_cuda/io/cloud_to_pcl.h>
-#include <pcl_cuda/io/debayering.h>
-#include <pcl_cuda/time_cpu.h>
+#include <pcl/memory.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/debayering.h>
+#include <pcl/cuda/time_cpu.h>
#include <pcl/io/kinect_grabber.h>
#include <opencv2/opencv.hpp>
-#include <boost/shared_ptr.hpp>
-
#include <functional>
#include <iostream>
#include <mutex>
+
class SimpleKinectTool
{
public:
std::function<void (const openni_wrapper::Image::Ptr& image)> f = std::bind (&SimpleKinectTool::cloud_cb_, this, _1);
- boost::signals2::connection c = interface->registerCallback (f);
+ interface->registerCallback (f);
interface->start ();
*
*/
-#include <pcl_cuda/io/cloud_to_pcl.h>
-#include <pcl_cuda/io/disparity_to_cloud.h>
-#include <pcl_cuda/sample_consensus/sac_model_plane.h>
-#include <pcl_cuda/sample_consensus/ransac.h>
-#include <pcl_cuda/time_cpu.h>
-
-#include <pcl/io/openni_grabber.h>
+#include <pcl/memory.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+#include <pcl/cuda/sample_consensus/ransac.h>
+#include <pcl/cuda/sample_consensus/sac_model_plane.h>
+#include <pcl/cuda/time_cpu.h>
+#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <boost/shared_ptr.hpp>
-
#include <functional>
#include <iostream>
#include <mutex>
+
class SimpleKinectTool
{
public:
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&SimpleKinectTool::cloud_cb_, this, _1, _2, _3);
- boost::signals2::connection c = interface->registerCallback (f);
+ interface->registerCallback (f);
//viewer.runOnVisualizationThread (fn, "viz_cb");
interface->start ();
*
*/
-#include <pcl_cuda/io/cloud_to_pcl.h>
-#include <pcl_cuda/io/extract_indices.h>
-#include <pcl_cuda/io/disparity_to_cloud.h>
-#include <pcl_cuda/sample_consensus/sac_model_1point_plane.h>
-#include <pcl_cuda/sample_consensus/multi_ransac.h>
-#include <pcl_cuda/segmentation/connected_components.h>
-#include <pcl_cuda/time_cpu.h>
-#include <pcl_cuda/time_gpu.h>
-
+#include <pcl/memory.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <pcl/common/transform.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+#include <pcl/cuda/io/extract_indices.h>
+#include <pcl/cuda/sample_consensus/multi_ransac.h>
+#include <pcl/cuda/sample_consensus/sac_model_1point_plane.h>
+#include <pcl/cuda/segmentation/connected_components.h>
+#include <pcl/cuda/time_cpu.h>
+#include <pcl/cuda/time_gpu.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#include <pcl/visualization/point_cloud_handlers.h>
-#include <opencv2/opencv.hpp>
-#include <opencv2/gpu/gpu.hpp>
+#include <boost/filesystem.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <boost/filesystem.hpp>
-#include <boost/shared_ptr.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/gpu/gpu.hpp>
#include <functional>
#include <iostream>
#include <mutex>
+
using namespace pcl_cuda;
template <template <typename> class Storage>
//cudaDeviceSetCacheConfig (cudaFuncCachePreferL1);
pcl::OpenNIGrabber interface {};
- boost::signals2::connection c;
if (use_device)
{
std::cerr << "[RANSAC] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&MultiRansac::cloud_cb<pcl_cuda::Device>, this, _1, _2, _3);
- c = interface.registerCallback (f);
+ interface.registerCallback (f);
}
else
{
std::cerr << "[RANSAC] Using CPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&MultiRansac::cloud_cb<pcl_cuda::Host>, this, _1, _2, _3);
- c = interface.registerCallback (f);
+ interface.registerCallback (f);
}
if (use_viewer)
*
*/
+#include <pcl/memory.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <pcl/cuda/features/normal_3d.h>
#include <pcl/cuda/time_cpu.h>
#include <pcl/cuda/time_gpu.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/pcl_macros.h>
#include <opencv2/opencv.hpp>
#include <opencv2/gpu/gpu.hpp>
-#include <boost/shared_ptr.hpp>
-
#include <functional>
#include <iostream>
#include <mutex>
+
using namespace pcl::cuda;
class NormalEstimation
//cudaDeviceSetCacheConfig (cudaFuncCachePreferL1);
pcl::OpenNIGrabber interface {};
- boost::signals2::connection c;
if (use_device)
{
std::cerr << "[RANSAC] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&MultiRansac::cloud_cb<Device>, this, _1, _2, _3);
- c = interface.registerCallback (f);
+ interface.registerCallback (f);
}
else
{
std::cerr << "[RANSAC] Using CPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&MultiRansac::cloud_cb<Host>, this, _1, _2, _3);
- c = interface.registerCallback (f);
+ interface.registerCallback (f);
}
if (use_viewer)
*
*/
-#include <pcl_cuda/time_cpu.h>
-#include <pcl_cuda/time_gpu.h>
-#include <pcl_cuda/io/cloud_to_pcl.h>
-#include <pcl_cuda/io/extract_indices.h>
-#include <pcl_cuda/io/disparity_to_cloud.h>
-#include <pcl_cuda/io/host_device.h>
-#include <pcl_cuda/sample_consensus/sac_model_1point_plane.h>
-#include <pcl_cuda/sample_consensus/ransac.h>
-
+#include <pcl/memory.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/cuda/time_cpu.h>
+#include <pcl/cuda/time_gpu.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+#include <pcl/cuda/io/extract_indices.h>
+#include <pcl/cuda/io/host_device.h>
+#include <pcl/cuda/sample_consensus/ransac.h>
+#include <pcl/cuda/sample_consensus/sac_model_1point_plane.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
-#include <boost/shared_ptr.hpp>
#include <functional>
#include <iostream>
#include <mutex>
+
using namespace pcl_cuda;
class SimpleKinectTool
}
else
{
- typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr inliers_stencil;
- inliers_stencil = sac.getInliersStencil ();
+ //typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr inliers_stencil;
+ //inliers_stencil = sac.getInliersStencil ();
// OpenNIRGB color;
// color.r = 253; color.g = 0; color.b = 0;
run (bool use_device)
{
#if 1
- pcl::Grabber* filegrabber = 0;
float frames_per_second = 1;
bool repeat = false;
*
*/
+#include <pcl/memory.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <pcl/cuda/features/normal_3d.h>
#include <pcl/cuda/time_cpu.h>
#include <pcl/cuda/time_gpu.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/pcl_macros.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/gpu/gpu.hpp>
-#include <boost/shared_ptr.hpp>
-
#include <functional>
#include <iostream>
#include <mutex>
+
using namespace pcl::cuda;
template <template <typename> class Storage>
}
std::vector<int> planes_inlier_counts = sac.getAllInlierCounts ();
std::vector<float4> coeffs = sac.getAllModelCoefficients ();
- std::vector<float3> centroids = sac.getAllModelCentroids ();
std::cerr << "Found " << planes_inlier_counts.size () << " planes" << std::endl;
for (unsigned int i = 0; i < planes.size (); i++)
{
if (use_file)
{
- pcl::Grabber* filegrabber = 0;
float frames_per_second = 1;
bool repeat = false;
{
pcl::OpenNIGrabber grabber {};
- boost::signals2::connection c;
if (use_device)
{
std::cerr << "[Segmentation] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&Segmentation::cloud_cb<Device>, this, _1, _2, _3);
- c = grabber.registerCallback (f);
+ grabber.registerCallback (f);
}
else
{
// std::cerr << "[Segmentation] Using CPU..." << std::endl;
// std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&Segmentation::cloud_cb<Host>, this, _1, _2, _3);
-// c = grabber.registerCallback (f);
+// grabber.registerCallback (f);
}
viewer.runOnVisualizationThread (std::bind(&Segmentation::viz_cb, this, _1), "viz_cb");
*
*/
-#include <pcl/cuda/features/normal_3d.h>
+#include <pcl/memory.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <pcl/cuda/time_cpu.h>
#include <pcl/cuda/time_gpu.h>
+#include <pcl/cuda/features/normal_3d.h>
#include <pcl/cuda/io/cloud_to_pcl.h>
-#include <pcl/cuda/io/extract_indices.h>
#include <pcl/cuda/io/disparity_to_cloud.h>
+#include <pcl/cuda/io/extract_indices.h>
#include <pcl/cuda/io/host_device.h>
#include <pcl/cuda/segmentation/connected_components.h>
#include <pcl/cuda/segmentation/mssegmentation.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/gpu/gpu.hpp>
-#include <boost/shared_ptr.hpp>
-
-#include <iostream>
#include <fstream>
#include <functional>
+#include <iostream>
#include <mutex>
+
using namespace pcl::cuda;
template <template <typename> class Storage>
{
pcl::OpenNIGrabber grabber {};
- boost::signals2::connection c;
if (use_device)
{
std::cerr << "[Segmentation] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&Segmentation::cloud_cb<Device>, this, _1, _2, _3);
- c = grabber.registerCallback (f);
+ grabber.registerCallback (f);
}
else
{
// std::cerr << "[Segmentation] Using CPU..." << std::endl;
// std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&Segmentation::cloud_cb<Host>, this, _1, _2, _3);
-// c = grabber.registerCallback (f);
+// grabber.registerCallback (f);
}
viewer.runOnVisualizationThread (std::bind(&Segmentation::viz_cb, this, _1), "viz_cb");
*
*/
+#include <pcl/memory.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <pcl/cuda/io/cloud_to_pcl.h>
#include <pcl/cuda/io/disparity_to_cloud.h>
#include <pcl/cuda/time_cpu.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
-#include <boost/shared_ptr.hpp>
#include <functional>
#include <iostream>
#include <mutex>
+
using pcl::cuda::PointCloudAOS;
using pcl::cuda::Device;
using pcl::cuda::Host;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&SimpleKinectTool::cloud_cb_, this, _1, _2, _3);
- boost::signals2::connection c = interface->registerCallback (f);
+ interface->registerCallback (f);
//viewer.runOnVisualizationThread (fn, "viz_cb");
interface->start ();
*
*/
+#include <pcl/memory.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
#include <pcl/cuda/io/cloud_to_pcl.h>
#include <pcl/cuda/io/disparity_to_cloud.h>
#include <pcl/cuda/time_cpu.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
-#include <boost/shared_ptr.hpp>
#include <functional>
#include <iostream>
#include <mutex>
+
using pcl::cuda::PointCloudAOS;
using pcl::cuda::Device;
inline __host__ __device__ RGB (char _r, char _g, char _b, char _alpha) :
r(_r), g(_g), b(_b), alpha(_alpha) {}
- inline __host__ __device__ bool operator == (const RGB &rhs)
+ inline __host__ __device__ bool operator == (const RGB &rhs) const
{
return (r == rhs.r && g == rhs.g && b == rhs.b && alpha == rhs.alpha);
}
#pragma once
-#include <boost/shared_ptr.hpp>
+#include <pcl/memory.h>
#include <pcl/cuda/point_cloud.h>
+
namespace pcl
{
namespace cuda
#include <pcl/cuda/point_types.h>
#include <pcl/cuda/thrust.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
namespace pcl
{
if ((int)nearestNeighbors.size () == k_arg)
{
double squared_radius;
- unsigned int pointCountRadiusSearch;
- unsigned int pointCountCircleSearch;
squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
leftY *=leftY;
rightY *= rightY;
- pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY);
-
// search for maximum distance between search point to window borders in 2-D search window
int maxSearchDistance = 0;
maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
maxSearchDistance +=1;
maxSearchDistance *=maxSearchDistance;
- pointCountCircleSearch= (int)(PI*(double)(maxSearchDistance*maxSearchDistance));
-
- if (1){//(pointCountCircleSearch<pointCountRadiusSearch) {
+ // check for nearest neighbors within window
+ while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
+ && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
+ {
+ // select point from organized point cloud
+ x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
+ y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
+ ++radiusSearchLookup_Iterator;
- // check for nearest neighbors within window
- while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
- && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
+ if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
{
- // select point from organized point cloud
- x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
- y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
- ++radiusSearchLookup_Iterator;
+ idx = y * (int)input_->width + x;
+ const PointT& point = input_->points[idx];
- if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
+ if ((point.x == point.x) && // check for NaNs
+ (point.y == point.y) && (point.z == point.z))
{
- idx = y * (int)input_->width + x;
- const PointT& point = input_->points[idx];
+ double squared_distance;
- if ((point.x == point.x) && // check for NaNs
- (point.y == point.y) && (point.z == point.z))
- {
- double squared_distance;
+ const double point_dist_x = point.x - p_q_arg.x;
+ const double point_dist_y = point.y - p_q_arg.y;
+ const double point_dist_z = point.z - p_q_arg.z;
- const double point_dist_x = point.x - p_q_arg.x;
- const double point_dist_y = point.y - p_q_arg.y;
- const double point_dist_z = point.z - p_q_arg.z;
+ // calculate squared distance
+ squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
+ * point_dist_z);
- // calculate squared distance
- squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
- * point_dist_z);
-
- if ( squared_distance<=squared_radius )
- {
- // add candidate
- nearestNeighborCandidate newCandidate;
- newCandidate.index_ = idx;
- newCandidate.squared_distance_ = squared_distance;
+ if ( squared_distance<=squared_radius )
+ {
+ // add candidate
+ nearestNeighborCandidate newCandidate;
+ newCandidate.index_ = idx;
+ newCandidate.squared_distance_ = squared_distance;
- nearestNeighbors.push_back (newCandidate);
- }
+ nearestNeighbors.push_back (newCandidate);
}
}
}
- } else {
- std::vector<int> k_radius_indices;
- std::vector<float> k_radius_distances;
-
- nearestNeighbors.clear();
-
- k_radius_indices.reserve (k_arg*2);
- k_radius_distances.reserve (k_arg*2);
-
- radiusSearch (p_q_arg, sqrt(squared_radius),k_radius_indices , k_radius_distances);
-
- std::cout << k_radius_indices.size () <<std::endl;
-
- for (std::size_t i = 0; i < k_radius_indices.size (); i++)
- {
- nearestNeighborCandidate newCandidate;
- newCandidate.index_ = k_radius_indices[i];
- newCandidate.squared_distance_ = k_radius_distances[i];
-
- nearestNeighbors.push_back (newCandidate);
- }
}
std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
endif()
add_custom_target(advanced ALL
- COMMAND "${SPHINX_EXECUTABLE}" -b html -a -d "${SPHINX_CACHE_DIR}" -D html_file_suffix=".${SPHINX_HTML_FILE_SUFFIX}" "${CMAKE_CURRENT_SOURCE_DIR}/content" html)
+ COMMAND "${SPHINX_EXECUTABLE}"
+ -b html
+ -d "${SPHINX_CACHE_DIR}"
+ -Dversion="${PCL_VERSION_PRETTY}"
+ -Drelease="${PCL_VERSION_PRETTY}"
+ "${CMAKE_CURRENT_SOURCE_DIR}/content" html)
add_dependencies(advanced doc)
set_target_properties(advanced PROPERTIES FOLDER "Documentation (Advanced)")
install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html"
COMPONENT doc)
install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/content/files"
DESTINATION "${DOC_INSTALL_DIR}/advanced"
- COMPONENT doc)
\ No newline at end of file
+ COMPONENT doc)
+++ /dev/null
-<!DOCTYPE html>
-<html lang="en">
-<head>
-<title>Documentation - Point Cloud Library (PCL)</title>
-</head>
-{% extends "!layout.html" %}
-
-{% block extrahead %}
-<?php
-define('MODX_CORE_PATH', '/var/www/pointclouds.org/core/');
-define('MODX_CONFIG_KEY', 'config');
-
-require_once MODX_CORE_PATH.'config/'.MODX_CONFIG_KEY.'.inc.php';
-require_once MODX_CORE_PATH.'model/modx/modx.class.php';
-$modx = new modX();
-$modx->initialize('web');
-
-$snip = $modx->runSnippet("getSiteNavigation", array('id'=>5, 'phLevels'=>'sitenav.level0,sitenav.level1', 'showPageNav'=>'n'));
-$chunkOutput = $modx->getChunk("site-header", array('sitenav'=>$snip));
-$bodytag = str_replace("[[+showSubmenus:notempty=`", "", $chunkOutput);
-$bodytag = str_replace("`]]", "", $bodytag);
-echo $bodytag;
-echo "\n";
-?>
-<div id="pagetitle">
-<h1>Documentation</h1>
-<a id="donate" href="http://www.openperception.org/support/"><img src="/assets/images/donate-button.png" alt="Donate to the Open Perception foundation"/></a>
-</div>
-<div id="page-content">
-{% endblock %}
-
-{% block relbar1 %}{% endblock %}
-{% block relbar2 %}{% endblock %}
-{% block rootrellink %}{% endblock %}
-
-{% block sidebarsearch %}{% endblock %}
-
-{% block footer %}
-</div> <!-- #page-content -->
-
-<?php
-$chunkOutput = $modx->getChunk("site-footer");
-echo $chunkOutput;
-?>
-{% endblock %}
-</html>
-
# All configuration values have a default; values that are commented out
# serve to show the default.
-import sys, os
-
# -- General configuration -----------------------------------------------------
# Add any Sphinx extension module names here, as strings. They can be extensions
# coming with Sphinx (named 'sphinx.ext.*') or your custom ones.
extensions = ['sphinx.ext.imgmath', 'sphinxcontrib.doxylink.doxylink']
# Add any paths that contain templates here, relative to this directory.
-templates_path = ['_templates']
+# templates_path = ['_templates']
# The suffix of source filenames.
source_suffix = '.rst'
master_doc = 'index'
# General information about the project.
-project = u'PCL'
+project = u'Point Cloud Library'
copyright = ''
# The version info for the project you're documenting, acts as replacement for
# The theme to use for HTML and HTML Help pages. Major themes that come with
# Sphinx are currently 'default' and 'sphinxdoc'.
-html_theme = 'sphinxdoc'
+html_theme = 'sphinx_rtd_theme'
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
html_use_modindex = False
# If false, no index is generated.
-html_use_index = False
+html_use_index = True
# If true, the index is split into individual pages for each letter.
html_split_index = False
# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml").
html_file_suffix = '.html'
-
-html_sidebars = {
- '**': [],
- 'using/windows': [],
-}
-html_show_copyright = False
-html_show_sphinx = False
+html_show_copyright = True
+html_show_sphinx = True
html_add_permalinks = None
needs_sphinx = '1.0'
file_insertion_enabled = True
2.1. Namespaces
^^^^^^^^^^^^^^^
-In a header file, the contets of a namespace should be indented, e.g.:
+In both header and implementation files, namespaces are to be explicitly
+declared, and their contents should not be indented, like clang-format
+enforces in the Formatting CI job, e.g.:
.. code-block:: cpp
namespace pcl
{
- class Foo
- {
- ...
- };
- }
-
-In an implementation file, the namespace must be added to each individual
-method or function definition, e.g.:
-
-.. code-block:: cpp
- void
- pcl::Foo::bar ()
+ class Foo
{
...
+ };
+
}
int
exampleMethod (int example_arg);
-If multiple namespaces are declared within header files, always use **2
-spaces** to indent them, e.g.:
-
-.. code-block:: cpp
-
- namespace foo
- {
- namespace bar
- {
- void
- method (int my_var);
- }
- }
Class and struct members are indented by **2 spaces**. Access qualifiers (public, private and protected) are put at the
indentation level of the class body and members affected by these qualifiers are indented by one more level, i.e. 2 spaces. E.g.:
namespace foo
{
- class Bar
- {
- int i;
- public:
- int j;
- protected:
- void
- baz ();
- }
+
+ class Bar
+ {
+ int i;
+ public:
+ int j;
+ protected:
+ void
+ baz ();
+ };
}
2.6. Automatic code formatting
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-The following set of rules can be automatically used by various different IDEs,
-editors, etc.
+We currently use clang-format-10 as the tool for auto-formatting our C++ code.
+Please note that different versions of clang-format can result in slightly different outputs.
-2.6.1. Emacs
-""""""""""""
+The style rules mentioned in this document are enforced via `PCL's .clang-format file
+<https://github.com/PointCloudLibrary/pcl/blob/master/.clang-format>`_.
+The style files which were previously distributed should now be considered deprecated.
-You can use the following `PCL C/C++ style file
-<https://raw.githubusercontent.com/PointCloudLibrary/pcl/master/doc/advanced/content/files/pcl-c-style.el>`_,
-download it to some known location and then:
+For the integration of clang-format with various text editors and IDE's, refer to this `page
+<https://clang.llvm.org/docs/ClangFormat.html>`_.
-* open .emacs
-* add the following before any C/C++ custom hooks
+Details about the style options used can be found `here
+<https://clang.llvm.org/docs/ClangFormatStyleOptions.html>`_.
-::
+2.6.1. Script usage
+"""""""""""""""""""
- (load-file "/location/to/pcl-c-style.el")
- (add-hook 'c-mode-common-hook 'pcl-set-c-style)
+PCL also creates a build target 'format' to format the whitelisted directories using clang-format.
-2.6.2. Uncrustify
-"""""""""""""""""
+Command line usage:
-You can find a semi-finished config for `Uncrustify <http://uncrustify.sourceforge.net/>`_ `here
-<http://dev.pointclouds.org/attachments/download/537/uncrustify.cfg>`_
+.. code-block:: shell
-2.6.3 Eclipse
-"""""""""""""
+ $ make format
-| You can find a PCL code style file for Eclipse `on GitHub <https://github.com/PointCloudLibrary/pcl/tree/master/doc/advanced/content/files>`_.
-| To add the new formatting style go to: Windows > Preferences > C/C++ > Code Style > Formatter
-| To format portion of codes, select the code and press Ctrl + Shift + F.
-| If you want to format the whole code in your project go to the tree and right click on the project: Source > Format.
+2.7. Includes
+^^^^^^^^^^^^^
-Note that the Eclipse formatter style is configured to wrap all arguments in a function, feel free to re-arange the arguments if you feel the need; for example,
-this improves readability:
+For consistent usage, headers should be included in the following order with alphabetical grouping ensured:
-.. code-block:: cpp
+1. PCL headers
- int
- displayPoint (float x, float y, float z,
- float r, float g, float b
- );
+ i. All modular PCL includes, except main includes of common module.
+
+ Examples:
-This eclipse formatter fails to add a space before brackets when using PCL macros:
+ .. code-block:: cpp
-.. code-block:: cpp
+ #include <pcl/common/common.h>
+ #include <pcl/simulation/camera.h>
+ #include <pcl/ml/dt/decision_forest.h>
- PCL_ERROR("Text\n");
+ #. The main PCL includes of common module. These are the header files in the ``pcl/common/include/pcl/`` directory.
+
+ Examples:
-should be
+ .. code-block:: cpp
-.. code-block:: cpp
+ #include <pcl/memory.h>
+ #include <pcl/pcl_macros.h>
+ #include <pcl/point_cloud.h>
- PCL_ERROR ("Text\n");
+2. Major 3rd-Party components of tests and modules
-.. note::
+ i. gtest
+ #. boost
+ #. Eigen
+ #. flann
+3. Major 3rd-Party components of apps
+
+ i. Qt
+ #. ui-files
+ #. vtk
+4. Minor 3rd-Party components
+
+ i. librealsense
+ #. ros/message_filters
+ #. opencv/opencv2
+ #. tide
+ #. thrust
+ #. OpenGL, GL & GLUT
+5. C++ standard library headers (alphabetical)
+6. Others
+
+This style can also be enforced via clang-format. For usage instructions, refer `2.6. Automatic code formatting`_.
- This style sheet is not perfect, please mention errors on the user mailing list and feel free to patch!
3. Structuring
==============
size.
* The output arguments will always be passed by reference.
+3.3. Object declaration
+^^^^^^^^^^^^^^^^^^^^^^^
+
+3.3.1 Use of auto
+"""""""""""""""""
+* For Iterators auto must be used as much as possible
+* In all the other cases auto can be used at the author's discretion
+* Use const auto references by default in range loops. Drop the const if the item needs to be modified.
+
+3.3.2 Type qualifiers of variables
+""""""""""""""""""""""""""""""""""
+* Declare variables const when they don't need to be modified.
+* Use const references whenever you don't need a copy of the variable.
+* Use of unsigned variables if the value is sure to not go negative by
+ use and by definition of the variable
set(STRIPPED_HEADERS "${PCL_SOURCE_DIR}/${PCL_MODULES_NAMES}/include")
string(REPLACE ";" "/include \\\n ${PCL_SOURCE_DIR}/" STRIPPED_HEADERS "${STRIPPED_HEADERS}")
-set(DOC_SOURCE_DIR "\"${PCL_SOURCE_DIR}\"\\")
+set(DOC_SOURCE_DIR "${PCL_SOURCE_DIR}")
file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html")
set(doxyfile "${CMAKE_CURRENT_BINARY_DIR}/doxyfile")
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/doxyfile.in" "${doxyfile}")
-add_custom_target(doc ALL "${DOXYGEN_EXECUTABLE}" "${doxyfile}")
+
+# The following process to use a stamp file is customized from the CMake 3.16.5 `doxygen_add_docs` command
+set(STAMP_FILE "${CMAKE_CURRENT_BINARY_DIR}/doc.stamp")
+add_custom_command(
+ VERBATIM
+ OUTPUT ${STAMP_FILE}
+ COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_CURRENT_BINARY_DIR}
+ COMMAND "${DOXYGEN_EXECUTABLE}" "${doxyfile}"
+ COMMAND ${CMAKE_COMMAND} -E touch ${STAMP_FILE}
+ WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}"
+ DEPENDS "${doxyfile}" ${DOC_SOURCE_DIR}
+ COMMENT "Generate stamp file for target doc"
+)
+add_custom_target(doc ALL
+ DEPENDS ${STAMP_FILE}
+ SOURCES ${DOC_SOURCE_DIR}
+)
+
set_target_properties(doc PROPERTIES FOLDER "Documentation (Doxygen)")
if(DOCUMENTATION_HTML_HELP STREQUAL YES)
install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html"
PROJECT_LOGO =
OUTPUT_DIRECTORY = "@CMAKE_CURRENT_BINARY_DIR@"
CREATE_SUBDIRS = NO
+ALLOW_UNICODE_NAMES = NO
OUTPUT_LANGUAGE = English
BRIEF_MEMBER_DESC = YES
REPEAT_BRIEF = YES
OPTIMIZE_OUTPUT_VHDL = NO
EXTENSION_MAPPING =
MARKDOWN_SUPPORT = YES
+TOC_INCLUDE_HEADINGS = 5
+AUTOLINK_SUPPORT = YES
BUILTIN_STL_SUPPORT = NO
CPP_CLI_SUPPORT = NO
SIP_SUPPORT = NO
IDL_PROPERTY_SUPPORT = YES
DISTRIBUTE_GROUP_DOC = NO
+GROUP_NESTED_COMPOUNDS = NO
SUBGROUPING = YES
INLINE_GROUPED_CLASSES = NO
INLINE_SIMPLE_STRUCTS = NO
INTERNAL_DOCS = NO
CASE_SENSE_NAMES = NO
HIDE_SCOPE_NAMES = NO
+HIDE_COMPOUND_REFERENCE= NO
SHOW_INCLUDE_FILES = YES
+SHOW_GROUPED_MEMB_INC = YES
FORCE_LOCAL_INCLUDES = NO
INLINE_INFO = YES
SORT_MEMBER_DOCS = YES
SORT_GROUP_NAMES = NO
SORT_BY_SCOPE_NAME = NO
STRICT_PROTO_MATCHING = NO
-GENERATE_TODOLIST = NO
+GENERATE_TODOLIST = YES
GENERATE_TESTLIST = NO
GENERATE_BUGLIST = NO
-GENERATE_DEPRECATEDLIST= NO
+GENERATE_DEPRECATEDLIST= YES
ENABLED_SECTIONS =
MAX_INITIALIZER_LINES = 30
SHOW_USED_FILES = YES
CITE_BIB_FILES =
#---------------------------------------------------------------------------
-# configuration options related to warning and progress messages
+# Configuration options related to warning and progress messages
#---------------------------------------------------------------------------
QUIET = YES
WARNINGS = YES
WARN_IF_UNDOCUMENTED = YES
WARN_IF_DOC_ERROR = YES
WARN_NO_PARAMDOC = NO
+WARN_AS_ERROR = NO
WARN_FORMAT = "$file:$line: $text "
WARN_LOGFILE =
#---------------------------------------------------------------------------
-# configuration options related to the input files
+# Configuration options related to the input files
#---------------------------------------------------------------------------
INPUT = "@PCL_SOURCE_DIR@" \
"@PCL_SOURCE_DIR@/doc/doxygen"
FILTER_PATTERNS =
FILTER_SOURCE_FILES = NO
FILTER_SOURCE_PATTERNS =
+USE_MDFILE_AS_MAINPAGE =
#---------------------------------------------------------------------------
-# configuration options related to source browsing
+# Configuration options related to source browsing
#---------------------------------------------------------------------------
SOURCE_BROWSER = YES
INLINE_SOURCES = NO
REFERENCED_BY_RELATION = YES
REFERENCES_RELATION = YES
REFERENCES_LINK_SOURCE = YES
+SOURCE_TOOLTIPS = YES
USE_HTAGS = NO
VERBATIM_HEADERS = YES
#---------------------------------------------------------------------------
-# configuration options related to the alphabetical class index
+# Configuration options related to the alphabetical class index
#---------------------------------------------------------------------------
ALPHABETICAL_INDEX = YES
COLS_IN_ALPHA_INDEX = 5
IGNORE_PREFIX =
#---------------------------------------------------------------------------
-# configuration options related to the HTML output
+# Configuration options related to the HTML output
#---------------------------------------------------------------------------
GENERATE_HTML = YES
HTML_OUTPUT = html
HTML_HEADER =
HTML_FOOTER = @PCL_SOURCE_DIR@/doc/doxygen/footer.html
HTML_STYLESHEET =
+HTML_EXTRA_STYLESHEET =
HTML_EXTRA_FILES =
HTML_COLORSTYLE_HUE = 87
HTML_COLORSTYLE_SAT = 46
HTML_COLORSTYLE_GAMMA = 73
HTML_TIMESTAMP = YES
HTML_DYNAMIC_SECTIONS = YES
+HTML_INDEX_NUM_ENTRIES = 100
GENERATE_DOCSET = YES
DOCSET_FEEDNAME = "Doxygen generated docs"
DOCSET_BUNDLE_ID = pointclouds.org
FORMULA_FONTSIZE = 10
FORMULA_TRANSPARENT = YES
USE_MATHJAX = NO
+MATHJAX_FORMAT = HTML-CSS
MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
MATHJAX_EXTENSIONS =
+MATHJAX_CODEFILE =
SEARCHENGINE = @DOCUMENTATION_SEARCHENGINE@
SERVER_BASED_SEARCH = NO
+EXTERNAL_SEARCH = NO
+SEARCHENGINE_URL =
+SEARCHDATA_FILE = searchdata.xml
+EXTERNAL_SEARCH_ID =
+EXTRA_SEARCH_MAPPINGS =
#---------------------------------------------------------------------------
-# configuration options related to the LaTeX output
+# Configuration options related to the LaTeX output
#---------------------------------------------------------------------------
GENERATE_LATEX = YES
LATEX_OUTPUT = latex
EXTRA_PACKAGES = amsmath amssymb
LATEX_HEADER =
LATEX_FOOTER =
+LATEX_EXTRA_STYLESHEET =
+LATEX_EXTRA_FILES =
PDF_HYPERLINKS = YES
USE_PDFLATEX = YES
LATEX_BATCHMODE = NO
LATEX_HIDE_INDICES = NO
LATEX_SOURCE_CODE = NO
LATEX_BIB_STYLE = plain
+LATEX_TIMESTAMP = NO
#---------------------------------------------------------------------------
-# configuration options related to the RTF output
+# Configuration options related to the RTF output
#---------------------------------------------------------------------------
GENERATE_RTF = NO
RTF_OUTPUT = rtf
RTF_HYPERLINKS = NO
RTF_STYLESHEET_FILE =
RTF_EXTENSIONS_FILE =
+RTF_SOURCE_CODE = NO
#---------------------------------------------------------------------------
-# configuration options related to the man page output
+# Configuration options related to the man page output
#---------------------------------------------------------------------------
GENERATE_MAN = NO
MAN_OUTPUT = man
MAN_EXTENSION = .3
+MAN_SUBDIR =
MAN_LINKS = NO
#---------------------------------------------------------------------------
-# configuration options related to the XML output
+# Configuration options related to the XML output
#---------------------------------------------------------------------------
GENERATE_XML = NO
XML_OUTPUT = xml
XML_PROGRAMLISTING = YES
#---------------------------------------------------------------------------
-# configuration options for the AutoGen Definitions output
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+GENERATE_DOCBOOK = NO
+DOCBOOK_OUTPUT = docbook
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
#---------------------------------------------------------------------------
GENERATE_AUTOGEN_DEF = NO
#---------------------------------------------------------------------------
-# configuration options related to the Perl module output
+# Configuration options related to the Perl module output
#---------------------------------------------------------------------------
GENERATE_PERLMOD = NO
PERLMOD_LATEX = NO
"HAVE_DAVIDSDK=1" \
"HAVE_DSSDK=1" \
"HAVE_RSSDK=1" \
- "DOXYGEN_ONLY=1"
+ "DOXYGEN_ONLY=1" \
+ "_WIN32=1" \
+ "PCL_DEPRECATED(major,minor,message)=/** \deprecated Scheduled for removal in version major.\ minor: message */"
EXPAND_AS_DEFINED =
SKIP_FUNCTION_MACROS = YES
#---------------------------------------------------------------------------
-# Configuration::additions related to external references
+# Configuration options related to external references
#---------------------------------------------------------------------------
TAGFILES =
#TAGFILES = qtools_docs/qtools.tag=../../qtools_docs/html
GENERATE_TAGFILE = pcl.tag
ALLEXTERNALS = NO
EXTERNAL_GROUPS = YES
-PERL_PATH = /usr/bin/perl
+EXTERNAL_PAGES = YES
#---------------------------------------------------------------------------
# Configuration options related to the dot tool
#---------------------------------------------------------------------------
CLASS_DIAGRAMS = YES
-MSCGEN_PATH =
+DIA_PATH =
HIDE_UNDOC_RELATIONS = YES
HAVE_DOT = @HAVE_DOT@
DOT_NUM_THREADS = 0
DOT_PATH =
DOTFILE_DIRS =
MSCFILE_DIRS =
-DOT_GRAPH_MAX_NODES = 50
+DIAFILE_DIRS =
+PLANTUML_JAR_PATH =
+PLANTUML_CFG_FILE =
+PLANTUML_INCLUDE_PATH =
+DOT_GRAPH_MAX_NODES = 250
MAX_DOT_GRAPH_DEPTH = 0
DOT_TRANSPARENT = YES
DOT_MULTI_TARGETS = NO
<h1>Quick Links</h1>
<ul>
- <li>Main website: http://www.pointclouds.org</li>
- <li>Developer Zone: https://github.com/PointCloudLibrary</li>
- <li>Build farm: https://travis-ci.org/PointCloudLibrary/pcl</li>
+ <li>Main Website: http://www.pointclouds.org</li>
+ <li>GitHub Repository: https://github.com/PointCloudLibrary</li>
+ <li>Continuous Integration: https://dev.azure.com/PointCloudLibrary/pcl/_build</li>
+ <li>Changelog: https://pointcloudlibrary.github.io/documentation/changelog.html</li>
</ul>
<h1>References</h1>
--- /dev/null
+sphinx
+sphinxcontrib-doxylink
endif()
add_custom_target(tutorials ALL
- COMMAND "${SPHINX_EXECUTABLE}" -b html -a -d "${SPHINX_CACHE_DIR}" -D html_file_suffix=".${SPHINX_HTML_FILE_SUFFIX}" "${CMAKE_CURRENT_SOURCE_DIR}/content" html)
+ COMMAND "${SPHINX_EXECUTABLE}"
+ -b html
+ -d "${SPHINX_CACHE_DIR}"
+ -Dversion="${PCL_VERSION_PRETTY}"
+ -Drelease="${PCL_VERSION_PRETTY}"
+ "${CMAKE_CURRENT_SOURCE_DIR}/content" html)
add_dependencies(tutorials doc)
set_target_properties(tutorials PROPERTIES FOLDER "Documentation (Tutorials)")
install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html"
+++ /dev/null
-<!DOCTYPE html>
-<html lang="en">
-<head>
-<title>Documentation - Point Cloud Library (PCL)</title>
-</head>
-{% extends "!layout.html" %}
-
-{% block extrahead %}
-<?php
-define('MODX_CORE_PATH', '/var/www/pointclouds.org/core/');
-define('MODX_CONFIG_KEY', 'config');
-
-require_once MODX_CORE_PATH.'config/'.MODX_CONFIG_KEY.'.inc.php';
-require_once MODX_CORE_PATH.'model/modx/modx.class.php';
-$modx = new modX();
-$modx->initialize('web');
-
-$snip = $modx->runSnippet("getSiteNavigation", array('id'=>5, 'phLevels'=>'sitenav.level0,sitenav.level1', 'showPageNav'=>'n'));
-$chunkOutput = $modx->getChunk("site-header", array('sitenav'=>$snip));
-$bodytag = str_replace("[[+showSubmenus:notempty=`", "", $chunkOutput);
-$bodytag = str_replace("`]]", "", $bodytag);
-echo $bodytag;
-echo "\n";
-?>
-<div id="pagetitle">
-<h1>Documentation</h1>
-<a id="donate" href="http://www.openperception.org/support/"><img src="/assets/images/donate-button.png" alt="Donate to the Open Perception foundation"/></a>
-</div>
-<div id="page-content">
-{% endblock %}
-
-{% block relbar1 %}{% endblock %}
-{% block relbar2 %}{% endblock %}
-{% block rootrellink %}{% endblock %}
-
-{% block sidebarsearch %}{% endblock %}
-
-{% block footer %}
-</div> <!-- #page-content -->
-
-<?php
-$chunkOutput = $modx->getChunk("site-footer");
-echo $chunkOutput;
-?>
-{% endblock %}
-</html>
-
The current document explains not only how to add your own `PointT` point type,
but also what templated point types are in PCL, why do they exist, and how are
they exposed. If you're already familiar with this information, feel free to
-skip to the last part of the document.
+skip to the last part of the document.
.. contents::
addition, the type that you want, might already be defined for you.
* `PointXYZ` - Members: float x, y, z;
-
+
This is one of the most used data types, as it represents 3D xyz information
only. The 3 floats are padded with an additional float for SSE alignment. The
user can either access `points[i].data[0]` or `points[i].x` for accessing
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
`intensity` a member of the same structure, as its contents will be
overwritten. For example, a dot product between two points will set their 4th
component to 0, otherwise the dot product doesn't make sense, etc.
-
+
Therefore for SSE-alignment, we pad intensity with 3 extra floats.
Inefficient in terms of storage, but good in terms of memory alignment.
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
.. code-block:: cpp
- union
+ union
{
float data_n[4];
float normal[3];
- struct
+ struct
{
float normal_x;
float normal_y;
float normal_z;
};
}
- union
+ union
{
- struct
+ struct
{
float curvature;
};
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
float z;
};
};
- union
+ union
{
float data_n[4];
float normal[3];
- struct
+ struct
{
float normal_x;
float normal_y;
float normal_z;
};
};
- union
+ union
{
- struct
+ struct
{
float curvature;
};
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
float z;
};
};
- union
+ union
{
float data_n[4];
float normal[3];
- struct
+ struct
{
float normal_x;
float normal_y;
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
float z;
};
};
- union
+ union
{
float data_n[4];
float normal[3];
- struct
+ struct
{
float normal_x;
float normal_y;
float normal_z;
};
}
- union
+ union
{
- struct
+ struct
{
float intensity;
float curvature;
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
* `MomentInvariants` - float j1, j2, j3;
Simple point type holding the 3 moment invariants at a surface patch. See
- `MomentInvariantsEstimation` for more information.
+ `MomentInvariantsEstimation` for more information.
.. code-block:: cpp
* `PrincipalRadiiRSD` - float r_min, r_max;
Simple point type holding the 2 RSD radii at a surface patch. See
- `RSDEstimation` for more information.
+ `RSDEstimation` for more information.
.. code-block:: cpp
* `Boundary` - std::uint8_t boundary_point;
Simple point type holding whether the point is lying on a surface boundary or
- not. See `BoundaryEstimation` for more information.
+ not. See `BoundaryEstimation` for more information.
.. code-block:: cpp
* `PrincipalCurvatures` - float principal_curvature[3], pc1, pc2;
Simple point type holding the principal curvatures of a given point. See
- `PrincipalCurvaturesEstimation` for more information.
+ `PrincipalCurvaturesEstimation` for more information.
.. code-block:: cpp
* `PFHSignature125` - float pfh[125];
Simple point type holding the PFH (Point Feature Histogram) of a given point.
- See `PFHEstimation` for more information.
+ See `PFHEstimation` for more information.
.. code-block:: cpp
* `FPFHSignature33` - float fpfh[33];
Simple point type holding the FPFH (Fast Point Feature Histogram) of a given
- point. See `FPFHEstimation` for more information.
+ point. See `FPFHEstimation` for more information.
.. code-block:: cpp
* `VFHSignature308` - float vfh[308];
Simple point type holding the VFH (Viewpoint Feature Histogram) of a given
- point. See `VFHEstimation` for more information.
+ point. See `VFHEstimation` for more information.
.. code-block:: cpp
* `Narf36` - float x, y, z, roll, pitch, yaw; float descriptor[36];
Simple point type holding the NARF (Normally Aligned Radius Feature) of a given
- point. See `NARFEstimation` for more information.
+ point. See `NARFEstimation` for more information.
.. code-block:: cpp
* `BorderDescription` - int x, y; BorderTraits traits;
Simple point type holding the border type of a given point. See
- `BorderEstimation` for more information.
+ `BorderEstimation` for more information.
.. code-block:: cpp
* `IntensityGradient` - float gradient[3];
Simple point type holding the intensity gradient of a given point. See
- `IntensityGradientEstimation` for more information.
+ `IntensityGradientEstimation` for more information.
.. code-block:: cpp
struct
- {
+ {
union
{
float gradient[3];
float gradient_y;
float gradient_z;
};
- };
- };
+ };
+ };
* `Histogram` - float histogram[N];
struct
{
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
.. code-block:: cpp
- union
+ union
{
float data[4];
- struct
+ struct
{
float x;
float y;
float z;
};
};
- union
+ union
{
float data_n[4];
float normal[3];
- struct
+ struct
{
float normal_x;
float normal_y;
{
public:
void
- compute (const pcl::PointCloud<PointT> &input,
+ compute (const pcl::PointCloud<PointT> &input,
pcl::PointCloud<PointT> &output);
}
#include "foo.h"
template <typename PointT> void
- Foo::compute (const pcl::PointCloud<PointT> &input,
+ Foo::compute (const pcl::PointCloud<PointT> &input,
pcl::PointCloud<PointT> &output)
{
output = input;
.. code-block:: cpp
:linenos:
-
+
// foo.cpp
#include "pcl/point_types.h"
.. code-block:: cpp
:linenos:
-
+
// foo.cpp
#include "pcl/point_types.h"
.. code-block:: cpp
:linenos:
-
+
struct MyPointType
{
float test;
:linenos:
#define PCL_NO_PRECOMPILE
+ #include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
into smaller parts so that the overall processing time for :math:`P` is
significantly reduced. A simple data clustering approach in an Euclidean sense
can be implemented by making use of a 3D grid subdivision of the space using
-fixed width boxes, or more generally, an octree data structure. This particular
+fixed-width boxes, or more generally, an octree data structure. This particular
representation is very fast to build and is useful for situations where either
a volumetric representation of the occupied space is needed, or the data in
each resultant 3D box (or octree leaf) can be approximated with a different
neighbors and implement a clustering technique that is essentially similar to a
flood fill algorithm.
-Let's assume we have given a point cloud with a table and objects on top of it.
+Let's assume we are given a point cloud with a table and objects on top of it.
We want to find and segment the individual object point clusters lying on the
plane. Assuming that we use a Kd-tree structure for finding the nearest
neighbors, the algorithmic steps for that would be (from [RusuDissertation]_):
* *for every point* :math:`\boldsymbol{p}_i \in Q` *do:*
- * *search for the set* :math:`P^i_k` *of point neighbors of* :math:`\boldsymbol{p}_i` *in a sphere with radius* :math:`r < d_{th}`;
+ * *search for the set* :math:`P^k_i` *of point neighbors of* :math:`\boldsymbol{p}_i` *in a sphere with radius* :math:`r < d_{th}`;
* *for every neighbor* :math:`\boldsymbol{p}^k_i \in P^k_i`, *check if the point has already been processed, and if not add it to* :math:`Q`;
# All configuration values have a default; values that are commented out
# serve to show the default.
-import sys, os
-
# -- General configuration -----------------------------------------------------
# Add any Sphinx extension module names here, as strings. They can be extensions
# coming with Sphinx (named 'sphinx.ext.*') or your custom ones.
imgmath_dvipng_args = ['-gamma', '1.5', '-D', '110', '-bg', 'Transparent']
# Add any paths that contain templates here, relative to this directory.
-templates_path = ['_templates']
+# templates_path = ['_templates']
# The suffix of source filenames.
source_suffix = '.rst'
master_doc = 'index'
# General information about the project.
-project = u'PCL'
+project = u'Point Cloud Library'
copyright = ''
# The version info for the project you're documenting, acts as replacement for
# The theme to use for HTML and HTML Help pages. Major themes that come with
# Sphinx are currently 'default' and 'sphinxdoc'.
-html_theme = 'sphinxdoc'
+html_theme = 'sphinx_rtd_theme'
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
html_use_modindex = False
# If false, no index is generated.
-html_use_index = False
+html_use_index = True
# If true, the index is split into individual pages for each letter.
html_split_index = False
# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml").
html_file_suffix = '.html'
-
-html_sidebars = {
- '**': [],
- 'using/windows': [],
-}
-html_show_copyright = False
-html_show_sphinx = False
+html_show_copyright = True
+html_show_sphinx = True
html_add_permalinks = u''
needs_sphinx = u'1.1'
file_insertion_enabled = True
.. literalinclude:: sources/convex_hull_2d/convex_hull_2d.cpp
:language: cpp
- :lines: 48-51
+ :lines: 49-52
Compiling and running the program
Terminology
-----------
-For the reminder of this article, we will make certain abbreviations and
+For the remainder of this article, we will make certain abbreviations and
introduce certain notations, to simplify the in-text explanations. Please see
the table below for a reference on each of the terms used.
1. an entire point cloud dataset, given via **setInputCloud (PointCloudConstPtr &)** - **mandatory**
- Any feature estimation class with attempt to estimate a feature at **every** point in the given input cloud.
+ Any feature estimation class will attempt to estimate a feature at **every** point in the given input cloud.
2. a subset of a point cloud dataset, given via **setInputCloud (PointCloudConstPtr &)** and **setIndices (IndicesConstPtr &)** - **optional**
and save it somewhere to disk.
Then, create a file, let's say, ``concave_hull_2d.cpp`` or
-``convex_hull2d.cpp`` in your favorite editor and place the following inside:
+``convex_hull_2d.cpp`` in your favorite editor and place the following inside:
.. literalinclude:: sources/concave_hull_2d/concave_hull_2d.cpp
:language: cpp
.. toctree::
-
+
+
+Introduction
+-----------
+
The following links describe a set of basic PCL tutorials. Please note that
their source codes may already be provided as part of the PCL regular releases,
so check there before you start copy & pasting the code. The list of tutorials
As always, we would be happy to hear your comments and receive your
contributions on any tutorial.
-Table of contents
------------------
-
- * :ref:`basic_usage`
- * :ref:`advanced_usage`
- * :ref:`applications_tutorial`
- * :ref:`features_tutorial`
- * :ref:`filtering_tutorial`
- * :ref:`i_o`
- * :ref:`keypoints_tutorial`
- * :ref:`kdtree_tutorial`
- * :ref:`octree_tutorial`
- * :ref:`range_images`
- * :ref:`recognition_tutorial`
- * :ref:`registration_tutorial`
- * :ref:`sample_consensus`
- * :ref:`segmentation_tutorial`
- * :ref:`surface_tutorial`
- * :ref:`visualization_tutorial`
- * :ref:`gpu`
-
.. _basic_usage:
Basic Usage
Theoretical primer
------------------
-A k-d tree, or k-dimensional tree, is a data structure used in computer science for organizing some number of points in a space with k dimensions. It is a binary search tree with other constraints imposed on it. K-d trees are very useful for range and nearest neighbor searches. For our purposes we will generally only be dealing with point clouds in three dimensions, so all of our k-d trees will be three-dimensional. Each level of a k-d tree splits all children along a specific dimension, using a hyperplane that is perpendicular to the corresponding axis. At the root of the tree all children will be split based on the first dimension (i.e. if the first dimension coordinate is less than the root it will be in the left-sub tree and if it is greater than the root it will obviously be in the right sub-tree). Each level down in the tree divides on the next dimension, returning to the first dimension once all others have been exhausted. They most efficient way to build a k-d tree is to use a partition method like the one Quick Sort uses to place the median point at the root and everything with a smaller one dimensional value to the left and larger to the right. You then repeat this procedure on both the left and right sub-trees until the last trees that you are to partition are only composed of one element.
+A k-d tree, or k-dimensional tree, is a data structure used in computer science for organizing some number of points in a space with k dimensions. It is a binary search tree with other constraints imposed on it. K-d trees are very useful for range and nearest neighbor searches. For our purposes we will generally only be dealing with point clouds in three dimensions, so all of our k-d trees will be three-dimensional. Each level of a k-d tree splits all children along a specific dimension, using a hyperplane that is perpendicular to the corresponding axis. At the root of the tree all children will be split based on the first dimension (i.e. if the first dimension coordinate is less than the root it will be in the left-sub tree and if it is greater than the root it will obviously be in the right sub-tree). Each level down in the tree divides on the next dimension, returning to the first dimension once all others have been exhausted. The most efficient way to build a k-d tree is to use a partition method like the one Quick Sort uses to place the median point at the root and everything with a smaller one-dimensional value to the left and larger to the right. You then repeat this procedure on both the left and right sub-trees until the last trees that you are to partition are only composed of one element.
From [Wikipedia]_:
:align: center
:alt:
- This is a demonstration of hour the Nearest-Neighbor search works.
+ This is a demonstration of how the Nearest-Neighbor search works.
The code
--------
Given a geometric surface, it's usually trivial to infer the direction of the
normal at a certain point on the surface as the vector perpendicular to the
-surface in that point. However, since the point cloud datasets that we acquire
+surface at that point. However, since the point cloud datasets that we acquire
represent a set of point samples on the real surface, there are two
possibilities:
**k-neighborhood**).
The specifics of the nearest-neighbor estimation problem raise the question of
-the *right scale factor*: given a sampled point cloud dataset , what are the
+the *right scale factor*: given a sampled point cloud dataset, what are the
correct **k** (given via **pcl::Feature::setKSearch**) or **r** (given via
**pcl::Feature::setRadiusSearch**) values that should be used in determining
the set of nearest neighbors of a point?
representation. To better illustrate this issue, the figure below presents the
effects of selecting a smaller scale (i.e., small **r** or **k**) versus a
larger scale (i.e., large **r** or **k**). The left part of the figures depicts
-a reasonable well chosen scale factor, with estimated surface normals
+a reasonable well-chosen scale factor, with estimated surface normals
approximately perpendicular for the two planar surfaces and small edges
visible all across the table. If the scale factor however is too big (right
part), and thus the set of neighbors is larger covering points from adjacent
.. image:: images/passthrough_2.png
-Note that the coordinate axis are represented as red (x), green (y), and blue
+Note that the coordinate axes are represented as red (x), green (y), and blue
(z). The five points are represented with green as the points remaining after
filtering and red as the points that have been removed by the filter.
Plane model segmentation
-------------------------
-In this tutorial we will learn how do a simple plane segmentation of a set of
-points, that is find all the points within a point cloud that support a plane
+In this tutorial we will learn how to do a simple plane segmentation of a set of
+points, that is to find all the points within a point cloud that support a plane
model. This tutorial supports the :ref:`extract_indices` tutorial, presented in
the **filtering** section.
create the :pcl:`SACSegmentation <pcl::SACSegmentation>` object and set the model and method type.
-This is also where we specify the "distance threshold", which determines how close a point must be to the model
-in order to be considered an inlier.
+This is also where we specify the "distance threshold", which determines how close a point must be to the model in order to be considered an inlier.
In this tutorial, we will use the RANSAC method (pcl::SAC_RANSAC) as the robust estimator of choice.
Our decision is motivated by RANSAC's simplicity (other robust estimators use it as
a base and add additional, more complicated concepts). For more information
.. image:: images/planar_segmentation_2.png
-Note that the coordinate axis are represented as red (x), green (y), and blue
+Note that the coordinate axes are represented as red (x), green (y), and blue
(z). The points are represented with red as the outliers, and green as the
inliers of the plane model found.
:align: center
:height: 400px
-Again hit 'r' to scale and center the view and then click and drag with the mouse to rotate around the cloud. You can see there are no longer any points that do not lie with in the plane model in this PointCloud. Hit 'q' to exit the viewer and program.
+Again hit 'r' to scale and center the view and then click and drag with the mouse to rotate around the cloud. You can see there are no longer any points that do not lie within the plane model in this PointCloud. Hit 'q' to exit the viewer and program.
There is also an example using a sphere in this program. If you run it with::
$ ./random_sample_consensus -s
-It will generate and display a sphereical cloud and some outliers as well.
+It will generate and display a spherical cloud and some outliers as well.
.. image:: images/ransac_outliers_sphere.png
:align: center
coordinate_frame=CAMERA_FRAME tells the system that x is facing right, y downwards and the z axis is forward. An alternative would be LASER_FRAME, with x facing forward, y to the left and z upwards.
-For noiseLevel=0 the range image is created using a normal z-buffer. Yet if you want to average over points falling in the same cell you can use a higher value. 0.05 would mean, that all point with a maximum distance of 5cm to the closest point are used to calculate the range.
+For noiseLevel=0 the range image is created using a normal z-buffer. Yet if you want to average over points falling in the same cell you can use a higher value. 0.05 would mean, that all points with a maximum distance of 5cm to the closest point are used to calculate the range.
-If minRange is greater 0 all points that are closer will be ignored.
+If minRange is greater than 0, all points that are closer will be ignored.
-borderSize greater 0 will leave a border of unobserved points around the image when cropping it.
+borderSize greater than 0 will leave a border of unobserved points around the image when cropping it.
.. literalinclude:: sources/range_image_creation/range_image_creation.cpp
The remaining code creates the range image from the point cloud with the given parameters and outputs some information on the terminal.
-The range image is derived from the PointCloud class and its points have the members x,y,z and range. There are three kinds of points. Valid points have a real range greater zero. Unobserved points have x=y=z=NAN and range=-INFINITY. Far range points have x=y=z=NAN and range=INFINITY.
+The range image is derived from the PointCloud class and its points have the members x,y,z and range. There are three kinds of points. Valid points have a real range greater than zero. Unobserved points have x=y=z=NAN and range=-INFINITY. Far range points have x=y=z=NAN and range=INFINITY.
Compiling and running the program
---------------------------------
In this tutorial we will learn how to use the region growing algorithm implemented in the ``pcl::RegionGrowing`` class.
The purpose of the said algorithm is to merge the points that are close enough in terms of the smoothness constraint.
Thereby, the output of this algorithm is the set of clusters,
-were each cluster is a set of points that are considered to be a part of the same smooth surface.
+where each cluster is a set of points that are considered to be a part of the same smooth surface.
The work of this algorithm is based on the comparison of the angles between the points normals.
Theoretical Primer
---------------------------
-Let's take a look on how the algorithm works.
+Let's take a look at how the algorithm works.
First of all it sorts the points by their curvature value.
It needs to be done because the region begins its growth from the point that has the minimum curvature value.
The reason for this is that the point with the minimum curvature is located in the flat area (growth from the flattest area
allows to reduce the total number of segments).
-So we have the sorted cloud. Until there are unlabeled points in the cloud, algorithm picks up the point with minimum curvature value and starts the growth of the region. This process occurs as follows:
+So we have the sorted cloud. Until there are no unlabeled points in the cloud, the algorithm picks up the point with minimum curvature value and starts the growth of the region. This process occurs as follows:
* The picked point is added to the set called seeds.
- * For every seed point algorithm finds neighbouring points.
+ * For every seed point, the algorithm finds its neighbouring points.
- * Every neighbour is tested for the angle between its normal and normal of the current seed point. If the angle is less than threshold value
+ * Every neighbour is tested for the angle between its normal and normal of the current seed point. If the angle is less than the threshold value
then current point is added to the current region.
- * After that every neighbour is tested for the curvature value. If the curvature is less than threshold value then this point is added to the seeds.
+ * After that every neighbour is tested for the curvature value. If the curvature is less than the threshold value then this point is added to the seeds.
* Current seed is removed from the seeds.
If the seeds set becomes empty this means that the algorithm has grown the region and the process is repeated from the beginning.
:lines: 30-35
These lines are given only for example. You can safely comment this part. Insofar as ``pcl::RegionGrowing`` is derived from ``pcl::PCLBase``,
-it can work with indices. It means you can point that you need to segment only
-those points that are listed in the indices array instead of the whole point cloud.
+it can work with indices. It means you can instruct that you only segment those points that are listed in the indices array instead of the whole point cloud.
.. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
:language: cpp
:lines: 37-39
-You have finally reached the part where ``pcl::RegionGrowing`` is instantiated. It is a template class that have two parameters:
+You have finally reached the part where ``pcl::RegionGrowing`` is instantiated. It is a template class that has two parameters:
* PointT - type of points to use(in the given example it is ``pcl::PointXYZ``)
* NormalT - type of normals to use(in the given example it is ``pcl::Normal``)
After that minimum and maximum cluster sizes are set. It means that
-after the segmentation is done all clusters that have less points then was set as minimum(or have more than maximum) will be discarded.
+after the segmentation is done all clusters that have less points than minimum(or have more than maximum) will be discarded.
The default values for minimum and maximum are 1 and 'as much as possible' respectively.
.. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
:language: cpp
:lines: 45-46
-This two lines are most important part in the algorithm initialization, because they are responsible for the mentioned smoothness constraint.
+These two lines are the most important part in the algorithm initialization, because they are responsible for the mentioned smoothness constraint.
First method sets the angle in radians that will be used as the allowable range for the normals deviation.
-If the deviation between points normals is less than smoothness threshold then they are suggested to be in the same cluster
+If the deviation between points normals is less than the smoothness threshold then they are suggested to be in the same cluster
(new point - the tested one - will be added to the cluster).
-The second one is responsible for curvature threshold. If two points have a small normals deviation then the disparity between their curvatures is tested.
-And if this value is less than curvature threshold then the algorithm will continue the growth of the cluster using new added point.
+The second one is responsible for the curvature threshold. If two points have a small normals deviation then the disparity between their curvatures is tested.
+And if this value is less than the curvature threshold then the algorithm will continue the growth of the cluster using the newly added point.
.. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
:language: cpp
.. image:: images/region_growing_segmentation_2.jpg
:height: 200px
-On the last image you can see that the colored cloud has many red points. This means that these points belong to the clusters
+In the last image you can see that the colored cloud has many red points. This means that these points belong to the clusters
that were rejected, because they had too much/little points.
.. image:: images/region_growing_segmentation_3.jpg
Removing outliers using a Conditional or RadiusOutlier removal
--------------------------------------------------------------
-This document demonstrates how to remove outliers from a PointCloud using several different methods in the filter module. First we will look at how to use a ConditionalRemoval filter which removes all indices in the given input cloud that do not satisfy one or more given conditions. Then we will learn how to us a RadiusOutlierRemoval filter which removes all indices in it's input cloud that don't have at least some number of neighbors within a certain range.
+This document demonstrates how to remove outliers from a PointCloud using several different methods in the filter module. First we will look at how to use a ConditionalRemoval filter which removes all indices in the given input cloud that do not satisfy one or more given conditions. Then we will learn how to us a RadiusOutlierRemoval filter which removes all indices in its input cloud that don't have at least some number of neighbors within a certain range.
The code
--------
To approximate the surface defined by a local neighborhood of points
**p1**, **p2** ... **pk** at a point **q** we use a bivariate polynomial height function
-defined on a on a robustly computed reference plane.
+defined on a robustly computed reference plane.
.. raw:: html
The code
--------
-First, create a file, let's say, ``resampling.cpp`` in your favorite
+First, download the dataset `bun0.pcd
+<https://raw.githubusercontent.com/PointCloudLibrary/pcl/master/test/bun0.pcd>`_
+and save it somewhere to disk.
+
+Then, create a file, let's say, ``resampling.cpp`` in your favorite
editor, and place the following inside it:
add_executable (bspline_fitting bspline_fitting.cpp)
target_link_libraries (bspline_fitting ${PCL_LIBRARIES})
-
// Project the model inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
- proj.setIndices (inliers);
+ // proj.setIndices (inliers);
proj.setInputCloud (cloud_filtered);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
add_executable (conditional_euclidean_clustering conditional_euclidean_clustering.cpp)
target_link_libraries (conditional_euclidean_clustering ${PCL_LIBRARIES})
-
add_definitions(${PCL_DEFINITIONS})
add_executable (correspondence_grouping correspondence_grouping.cpp)
-target_link_libraries (correspondence_grouping ${PCL_LIBRARIES})
\ No newline at end of file
+target_link_libraries (correspondence_grouping ${PCL_LIBRARIES})
add_executable (cylinder_segmentation cylinder_segmentation.cpp)
target_link_libraries (cylinder_segmentation ${PCL_LIBRARIES})
-
-
add_executable (don_segmentation don_segmentation.cpp)
target_link_libraries (don_segmentation ${PCL_LIBRARIES})
-
-
#include <vector>
#include <pcl/common/common.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <pcl/console/print.h>
#include <pcl/io/ensenso_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
add_executable (extract_indices extract_indices.cpp)
target_link_libraries (extract_indices ${PCL_LIBRARIES})
-
-
add_executable (global_hypothesis_verification global_hypothesis_verification.cpp)
target_link_libraries (global_hypothesis_verification ${PCL_LIBRARIES})
-
+++ /dev/null
-if(NOT VTK_FOUND)
- set(DEFAULT FALSE)
- set(REASON "VTK was not found.")
-else(NOT VTK_FOUND)
- set(DEFAULT TRUE)
- set(REASON)
- set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE")
- include (${VTK_USE_FILE})
- include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
-endif(NOT VTK_FOUND)
-
-set(the_target people_tracking)
-
-include_directories(${VTK_INCLUDE_DIRS})
-
-PCL_ADD_EXECUTABLE(people_detect ${SUBSYS_NAME} src/people_detect.cpp)
-target_link_libraries (people_detect pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES})
-
sac_ia.setMaxCorrespondenceDistance (max_correspondence_distance);
sac_ia.setMaximumIterations (nr_iterations);
- sac_ia.setInputCloud (source_points);
+ sac_ia.setInputSource (source_points);
sac_ia.setSourceFeatures (source_descriptors);
sac_ia.setInputTarget (target_points);
PointCloudPtr source_points_transformed (new PointCloud);
pcl::transformPointCloud (*source_points, *source_points_transformed, initial_alignment);
- icp.setInputCloud (source_points_transformed);
+ icp.setInputSource (source_points_transformed);
icp.setInputTarget (target_points);
PointCloud registration_output;
std::vector<pcl::Vertices> faces;
};
-typedef boost::shared_ptr<Mesh> MeshPtr;
+using MeshPtr = std::shared_ptr<Mesh>;
PointCloudPtr
smoothPointCloud (const PointCloudPtr & input, float radius, int polynomial_order)
#include "openni_capture.h"
+
#include <pcl/io/pcd_io.h>
-#include <boost/make_shared.hpp>
+#include <pcl/memory.h> // for pcl::make_shared
+
#include <mutex>
OpenNICapture::OpenNICapture (const std::string& device_id)
{
mutex_.lock ();
++frame_counter_;
- most_recent_frame_ =
boost::make_shared<PointCloud> (*cloud); // Make a copy of the frame
+ most_recent_frame_ =
pcl::make_shared<PointCloud> (*cloud); // Make a copy of the frame
mutex_.unlock ();
}
#include <vector>
#include <string>
#include <sstream>
+
#include <pcl/io/pcd_io.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
# include <pcl/keypoints/harris_3d.h>
#endif
+#include <pcl/memory.h> // for pcl::dynamic_pointer_cast
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor,
typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target);
-
+
/**
* @brief starts the event loop for the visualizer
*/
* @param segmented the resulting segmented point cloud containing only points of the largest cluster
*/
void segmentation (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented) const;
-
+
/**
* @brief Detects key points in the input point cloud
* @param input the input point cloud
* @param keypoints the resulting key points. Note that they are not necessarily a subset of the input cloud
*/
void detectKeypoints (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints) const;
-
+
/**
* @brief extract descriptors for given key points
* @param input point cloud to be used for descriptor extraction
* @param features resulting descriptors
*/
void extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, typename pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints, typename pcl::PointCloud<FeatureType>::Ptr features);
-
+
/**
* @brief find corresponding features based on some metric
* @param source source feature descriptors
- * @param target target feature descriptors
+ * @param target target feature descriptors
* @param correspondences indices out of the target descriptors that correspond (nearest neighbor) to the source descriptors
- */
+ */
void findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const;
-
+
/**
* @brief remove non-consistent correspondences
*/
void filterCorrespondences ();
-
+
/**
* @brief calculate the initial rigid transformation from filtered corresponding keypoints
*/
void determineInitialTransformation ();
-
+
/**
* @brief calculate the final rigid transformation using ICP over all points
*/
* @param cookie user defined data passed during registration of the callback
*/
void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* cookie);
-
+
private:
pcl::visualization::PCLVisualizer visualizer_;
pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_;
, show_correspondences (false)
{
visualizer_.registerKeyboardCallback(&ICCVTutorial::keyboard_callback, *this, 0);
-
+
segmentation (source_, source_segmented_);
- segmentation (target_, target_segmented_);
-
+ segmentation (target_, target_segmented_);
+
detectKeypoints (source_segmented_, source_keypoints_);
detectKeypoints (target_segmented_, target_keypoints_);
-
+
extractDescriptors (source_segmented_, source_keypoints_, source_features_);
extractDescriptors (target_segmented_, target_keypoints_, target_features_);
-
+
findCorrespondences (source_features_, target_features_, source2target_);
findCorrespondences (target_features_, source_features_, target2source_);
-
+
filterCorrespondences ();
-
+
determineInitialTransformation ();
determineFinalTransformation ();
-
+
reconstructSurface ();
}
seg.setInputCloud (source);
seg.segment (*inliers, *coefficients);
-
+
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (source);
extract.setIndices (inliers);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*segmented, *segmented, indices);
std::cout << "OK" << std::endl;
-
+
std::cout << "clustering..." << std::flush;
// euclidean clustering
typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
clustering.setSearchMethod (tree);
clustering.setInputCloud(segmented);
clustering.extract (cluster_indices);
-
+
if (cluster_indices.size() > 0)//use largest cluster
{
std::cout << cluster_indices.size() << " clusters found";
{
typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(new pcl::PointCloud<pcl::PointXYZRGB>);
kpts->points.resize(keypoints->points.size());
-
+
pcl::copyPointCloud(*keypoints, *kpts);
-
- typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType>::Ptr feature_from_normals =
boost::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_);
-
+
+ typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType>::Ptr feature_from_normals =
pcl::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_);
+
feature_extractor_->setSearchSurface(input);
feature_extractor_->setInputCloud(kpts);
-
+
if (feature_from_normals)
- //if (
boost::dynamic_pointer_cast<typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_))
+ //if (
pcl::dynamic_pointer_cast<typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_))
{
std::cout << "normal estimation..." << std::flush;
typename pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
for (unsigned cIdx = 0; cIdx < source2target_.size (); ++cIdx)
if (target2source_[source2target_[cIdx]] == cIdx)
correspondences.push_back(std::make_pair(cIdx, source2target_[cIdx]));
-
+
correspondences_->resize (correspondences.size());
for (unsigned cIdx = 0; cIdx < correspondences.size(); ++cIdx)
{
(*correspondences_)[cIdx].index_query = correspondences[cIdx].first;
(*correspondences_)[cIdx].index_match = correspondences[cIdx].second;
}
-
+
pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZI> rejector;
rejector.setInputSource(source_keypoints_);
rejector.setInputTarget(target_keypoints_);
{
std::cout << "initial alignment..." << std::flush;
pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>::Ptr transformation_estimation (new pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI>);
-
+
transformation_estimation->estimateRigidTransformation (*source_keypoints_, *target_keypoints_, *correspondences_, initial_transformation_matrix_);
-
+
pcl::transformPointCloud(*source_segmented_, *source_transformed_, initial_transformation_matrix_);
std::cout << "OK" << std::endl;
}
{
std::cout << "final registration..." << std::flush;
pcl::Registration<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr registration (new pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB>);
- registration->setInputCloud(source_transformed_);
+ registration->setInputSource(source_transformed_);
//registration->setInputCloud(source_segmented_);
registration->setInputTarget (target_segmented_);
registration->setMaxCorrespondenceDistance(0.05);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr merged (new pcl::PointCloud<pcl::PointXYZRGB>);
*merged = *source_transformed_;
*merged += *target_segmented_;
-
+
// apply grid filtering to reduce amount of points as well as to make them uniform distributed
pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
voxel_grid.setInputCloud(merged);
normal_estimation.setRadiusSearch (0.01);
normal_estimation.setInputCloud (merged);
normal_estimation.compute (*vertices);
-
+
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
tree->setInputCloud (vertices);
visualizer_.addPointCloud(source_, "source_points");
}
break;
-
+
case '2':
if (!visualizer_.removePointCloud("target_points"))
{
visualizer_.addPointCloud(target_, "target_points");
}
break;
-
+
case '3':
if (!visualizer_.removePointCloud("source_segmented"))
{
visualizer_.addPointCloud(source_segmented_, "source_segmented");
}
break;
-
+
case '4':
if (!visualizer_.removePointCloud("target_segmented"))
{
visualizer_.addPointCloud(target_segmented_, "target_segmented");
}
break;
-
+
case '5':
if (!visualizer_.removePointCloud("source_keypoints"))
{
visualizer_.addPointCloud(source_keypoints_, keypoint_color, "source_keypoints");
}
break;
-
+
case '6':
if (!visualizer_.removePointCloud("target_keypoints"))
{
visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, source2target_, "source2target");
else
visualizer_.removeCorrespondences("source2target");
-
+
show_source2target_ = !show_source2target_;
break;
show_target2source_ = !show_target2source_;
break;
-
+
case '9':
if (!show_correspondences)
visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, *correspondences_, "correspondences");
visualizer_.removeCorrespondences("correspondences");
show_correspondences = !show_correspondences;
break;
-
+
case 'i':
case 'I':
if (!visualizer_.removePointCloud("transformed"))
if (!visualizer_.removePointCloud("registered"))
visualizer_.addPointCloud(source_registered_, "registered");
break;
-
+
case 't':
case 'T':
visualizer_.addPolygonMesh(surface_, "surface");
}
}
-int
+int
main (int argc, char ** argv)
{
- if (argc < 6)
+ if (argc < 6)
{
pcl::console::print_info ("Syntax is: %s <source-pcd-file> <target-pcd-file> <keypoint-method> <descriptor-type> <surface-reconstruction-method>\n", argv[0]);
pcl::console::print_info ("available <keypoint-methods>: 1 = Sift3D\n");
pcl::console::print_info (" 4 = PFHRGB\n\n");
pcl::console::print_info ("available <surface-methods>: 1 = Greedy Projection\n");
pcl::console::print_info (" 2 = Marching Cubes\n");
-
+
return (1);
}
pcl::console::print_info ("== MENU ==\n");
pcl::console::print_info ("t - show/hide triangulation (surface reconstruction)\n");
pcl::console::print_info ("h - show visualizer options\n");
pcl::console::print_info ("q - quit\n");
-
+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile (argv[1], *source);
-
+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr target (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile (argv[2], *target);
-
+
int keypoint_type = atoi (argv[3]);
int descriptor_type = atoi (argv[4]);
int surface_type = atoi (argv[5]);
-
+
pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector;
-
+
if (keypoint_type == 1)
{
pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>* sift3D = new pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>;
case 4:
harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::NOBLE);
break;
-
+
case 5:
harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::LOWE);
break;
exit (1);
break;
}
-
}
-
+
pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstruction;
-
+
if (surface_type == 1)
{
pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>* gp3 = new pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>;
pcl::console::print_error("unknown surface reconstruction method %d\n expecting values between 1 and 2", surface_type);
exit (1);
}
-
+
switch (descriptor_type)
{
case 1:
tutorial.run ();
}
break;
-
+
case 2:
{
pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>* shot = new pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>;
tutorial.run ();
}
break;
-
+
case 3:
{
pcl::Feature<pcl::PointXYZRGB, pcl::PFHSignature125>::Ptr feature_extractor (new pcl::PFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHSignature125>);
tutorial.run ();
}
break;
-
+
case 4:
{
pcl::Feature<pcl::PointXYZRGB, pcl::PFHRGBSignature250>::Ptr feature_extractor (new pcl::PFHRGBEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHRGBSignature250>);
tutorial.run ();
}
break;
-
+
default:
pcl::console::print_error("unknown descriptor type %d\n expecting values between 1 and 4", descriptor_type);
exit (1);
break;
- }
-
+ }
+
return (0);
}
add_executable (implicit_shape_model implicit_shape_model.cpp)
target_link_libraries (implicit_shape_model ${PCL_LIBRARIES})
-
-
sac_ia.setMaxCorrespondenceDistance (max_correspondence_distance);
sac_ia.setMaximumIterations (nr_iterations);
- sac_ia.setInputCloud (source_points);
+ sac_ia.setInputSource (source_points);
sac_ia.setSourceFeatures (source_descriptors);
sac_ia.setInputTarget (target_points);
PointCloudPtr source_points_transformed (new PointCloud);
pcl::transformPointCloud (*source_points, *source_points_transformed, initial_alignment);
- icp.setInputCloud (source_points_transformed);
+ icp.setInputSource (source_points_transformed);
icp.setInputTarget (target_points);
PointCloud registration_output;
std::vector<pcl::Vertices> faces;
};
-typedef boost::shared_ptr<Mesh> MeshPtr;
+using MeshPtr = std::shared_ptr<Mesh>;
PointCloudPtr
smoothPointCloud (const PointCloudPtr & input, float radius, int polynomial_order)
std::vector<pcl::Vertices> faces;
};
-typedef boost::shared_ptr<Mesh> MeshPtr;
+using MeshPtr = std::shared_ptr<Mesh>;
PointCloudPtr
smoothPointCloud (const PointCloudPtr & input, float radius, int polynomial_order)
#include "openni_capture.h"
+
#include <pcl/io/pcd_io.h>
+#include <pcl/memory.h> // for pcl::make_shared
+
#include <mutex>
-#include <boost/make_shared.hpp>
OpenNICapture::OpenNICapture (const std::string& device_id)
: grabber_ (device_id)
{
// Stop the grabber when shutting down
grabber_.stop ();
- if (preview_)
delete preview_;
}
{
mutex_.lock ();
++frame_counter_;
- most_recent_frame_ =
boost::make_shared<PointCloud> (*cloud); // Make a copy of the frame
+ most_recent_frame_ =
pcl::make_shared<PointCloud> (*cloud); // Make a copy of the frame
mutex_.unlock ();
}
int
main (int argc, char** argv)
{
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>(5,1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the CloudIn data
- cloud_in->width = 5;
- cloud_in->height = 1;
- cloud_in->is_dense = false;
- cloud_in->points.resize (cloud_in->width * cloud_in->height);
- for (std::size_t i = 0; i < cloud_in->points.size (); ++i)
+ for (auto& point : *cloud_in)
{
- cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
- cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
- cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
+ point.x = 1024 * rand() / (RAND_MAX + 1.0f);
+ point.y = 1024 * rand() / (RAND_MAX + 1.0f);
+ point.z = 1024 * rand() / (RAND_MAX + 1.0f);
}
- std::cout << "Saved " << cloud_in->points.size () << " data points to input:"
- << std::endl;
- for (std::size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << " " <<
- cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
- cloud_in->points[i].z << std::endl;
+
+ std::cout << "Saved " << cloud_in->points.size () << " data points to input:" << std::endl;
+
+ for (auto& point : *cloud_in)
+ std::cout << point << std::endl;
+
*cloud_out = *cloud_in;
+
std::cout << "size:" << cloud_out->points.size() << std::endl;
- for (std::size_t i = 0; i < cloud_in->points.size (); ++i)
- cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
- std::cout << "Transformed " << cloud_in->points.size () << " data points:"
- << std::endl;
- for (std::size_t i = 0; i < cloud_out->points.size (); ++i)
- std::cout << " " << cloud_out->points[i].x << " " <<
- cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
+ for (auto& point : *cloud_out)
+ point.x += 0.7f;
+
+ std::cout << "Transformed " << cloud_in->points.size () << " data points:" << std::endl;
+
+ for (auto& point : *cloud_out)
+ std::cout << point << std::endl;
+
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
+
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
+
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
add_executable (min_cut_segmentation min_cut_segmentation.cpp)
target_link_libraries (min_cut_segmentation ${PCL_LIBRARIES})
-
-
add_executable (moment_of_inertia moment_of_inertia.cpp)
target_link_libraries (moment_of_inertia ${PCL_LIBRARIES})
-
add_executable (narf_descriptor_visualization narf_descriptor_visualization.cpp)
target_link_libraries (narf_descriptor_visualization ${PCL_LIBRARIES})
-
add_executable (narf_feature_extraction narf_feature_extraction.cpp)
target_link_libraries (narf_feature_extraction ${PCL_LIBRARIES})
-
add_executable (narf_keypoint_extraction narf_keypoint_extraction.cpp)
target_link_libraries (narf_keypoint_extraction ${PCL_LIBRARIES})
-
add_executable (openni_range_image_visualization openni_range_image_visualization.cpp)
target_link_libraries (openni_range_image_visualization ${PCL_LIBRARIES})
-
/* \author Radu Bogdan Rusu
* adaptation Raphael Favier*/
-#include <boost/make_shared.hpp>
+#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance (0.1);
// Set the point representation
- reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
+ reg.setPointRepresentation (pcl::make_shared<const MyPointRepresentation> (point_representation));
reg.setInputSource (points_with_normals_src);
reg.setInputTarget (points_with_normals_tgt);
add_executable (region_growing_segmentation region_growing_segmentation.cpp)
target_link_libraries (region_growing_segmentation ${PCL_LIBRARIES})
-
Correspondences &remaining_correspondences)
{
CorrespondenceRejectorDistance rej;
- rej.setInput
Cloud<PointXYZ> (keypoints_src);
+ rej.setInput
Source<PointXYZ> (keypoints_src);
rej.setInputTarget<PointXYZ> (keypoints_tgt);
rej.setMaximumDistance (1); // 1m
rej.setInputCorrespondences (all_correspondences);
add_executable (rops_feature rops_feature.cpp)
target_link_libraries (rops_feature ${PCL_LIBRARIES})
-
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/point_types.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <fstream>
#include <vector>
#include <Eigen/Core>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
void
align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)
{
- sac_ia_.setInputCloud (template_cloud.getPointCloud ());
+ sac_ia_.setInputSource (template_cloud.getPointCloud ());
sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());
pcl::PointCloud<pcl::PointXYZ> registration_output;
reader.read ("table_scene_lms400.pcd", *cloud); // Remember to download the file first!
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
- << " data points (" << pcl::getFieldsList (*cloud) << ").";
+ << " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;
// Create the filtering object
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
- << " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";
+ << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;
pcl::PCDWriter writer;
writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered,
erroneous values, which in turn might cause point cloud registration failures.
Some of these irregularities can be solved by performing a statistical analysis
on each point's neighborhood, and trimming those which do not meet a certain
-criteria. Our sparse outlier removal is based on the computation of the
+criterion. Our sparse outlier removal is based on the computation of the
distribution of point to neighbors distances in the input dataset. For each
point, we compute the mean distance from it to all its neighbors. By assuming
that the resulted distribution is Gaussian with a mean and a standard
This tutorial gives an example of how some of the tools covered in the other tutorials can be combined to solve a higher level problem --- aligning a previously captured model of an object to some newly captured data. In this specific example, we'll take a depth image that contains a person and try to fit some previously captured templates of their face; this will allow us to determine the position and orientation of the face in the scene.
-.. raw:: html
+.. raw:: html
<iframe width="560" height="349" style="margin-left:50px" src="http://www.youtube.com/embed/1T5HxTTgE4I" frameborder="0" allowfullscreen></iframe>
-We can use the code below to fit a template of a person's face (the blue points) to a new point cloud (the green points).
+We can use the code below to fit a template of a person's face (the blue points) to a new point cloud (the green points).
The code
Now we'll examine the *TemplateAlignment* class, which as the name suggests, will be used to perform template alignment (also referred to as template fitting/matching/registration). A template is typically a small group of pixels or points that represents a known part of a larger object or scene. By registering a template to a new image or point cloud, you can determine the position and orientation of the object that the template represents.
-We start by defining a structure to store the alignment results. It contains a floating point value that represents the "fitness" of the alignment (a lower number means a better alignment) and a transformation matrix that describes how template points should be rotated and translated in order to best align with the points in the target cloud.
+We start by defining a structure to store the alignment results. It contains a floating point value that represents the "fitness" of the alignment (a lower number means a better alignment) and a transformation matrix that describes how template points should be rotated and translated in order to best align with the points in the target cloud.
.. note::
- Because we are including an Eigen::Matrix4f in this struct, we need to include the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro, which will overload the struct's "operator new" so that it will generate 16-bytes-aligned pointers. If you're curious, you can find more information about this issue `here <http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html>`_. For convenience, there is a redefinition of the macro in pcl_macros.h, aptly named PCL_MAKE_ALIGNED_OPERATOR_NEW which will let us for example call `pcl::make_shared` to create a `shared_ptr` of over-aligned classes.
+ Because we are including an Eigen::Matrix4f in this struct, we need to include the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro, which will overload the struct's "operator new" so that it will generate 16-bytes-aligned pointers. If you're curious, you can find more information about this issue `here <http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html>`_. For convenience, there is a redefinition of the macro in memory.h, aptly named PCL_MAKE_ALIGNED_OPERATOR_NEW which will let us for example call `pcl::make_shared` to create a `shared_ptr` of over-aligned classes.
.. literalinclude:: sources/template_alignment/template_alignment.cpp
:language: cpp
After a few seconds, you will see output similar to::
Best fitness score: 0.000009
-
- | 0.834 0.295 0.466 |
- R = | -0.336 0.942 0.006 |
- | -0.437 -0.162 0.885 |
-
+
+ | 0.834 0.295 0.466 |
+ R = | -0.336 0.942 0.006 |
+ | -0.437 -0.162 0.885 |
+
t = < -0.373, -0.097, 0.087 >
You can also use the `pcl_viewer <http://www.pointclouds.org/documentation/overview/visualization.php>`_ utility to visualize the aligned template and overlay it against the target cloud by running the following command::
.. image:: images/features_normal.jpg
- An example of two of the most widely used geometric point features are the underlying surface's estimated curvature and normal at a query point ``p``. Both of them are considered local features, as they characterize a point using the information provided by its ``k`` closest point neighbors. For determining these neighbors efficiently, the input dataset is usually split into smaller chunks using spatial decomposition techniques such as octrees or kD-trees (see the figure below - left: kD-tree, right: octree), and then closest point searches are performed in that space. Depending on the application one can opt for either determining a fixed number of ``k`` points in the vicinity of ``p``, or all points which are found inside of a sphere of radius ``r`` centered at ``p``. Unarguably, one the easiest methods for estimating the surface normals and curvature changes at a point ``p`` is to perform an eigendecomposition (i.e., compute the eigenvectors and eigenvalues) of the k-neighborhood point surface patch. Thus, the eigenvector corresponding to the smallest eigenvalue will approximate the surface normal ``n`` at point ``p``, while the surface curvature change will be estimated from the eigenvalues as:
-
- .. image:: images/form_0.png
-
- .. image:: images/form_1.png
-
- |
+ An example of two of the most widely used geometric point features are the underlying surface's estimated curvature and normal at a query point ``p``. Both of them are considered local features, as they characterize a point using the information provided by its ``k`` closest point neighbors. For determining these neighbors efficiently, the input dataset is usually split into smaller chunks using spatial decomposition techniques such as octrees or kD-trees, and then closest point searches are performed in that space. Depending on the application one can opt for either determining a fixed number of ``k`` points in the vicinity of ``p``, or all points which are found inside of a sphere of radius ``r`` centered at ``p``. Unarguably, one the easiest methods for estimating the surface normals and curvature changes at a point ``p`` is to perform an eigendecomposition (i.e., compute the eigenvectors and eigenvalues) of the k-neighborhood point surface patch. Thus, the eigenvector corresponding to the smallest eigenvalue will approximate the surface normal ``n`` at point ``p``, while the surface curvature change will be estimated from the eigenvalues as :math:`\frac{\lambda_0}{\lambda_0+\lambda_1+\lambda_2}` with :math:`\lambda_0<\lambda_1<\lambda_2`.
.. image:: images/features_bunny.jpg
__ Octree_
Top_
-
*
*/
-#include <iostream>
-#include <limits>
-
#include <pcl/point_types.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
int
main ()
* members of the source PointType.
*/
-// STL
#include <iostream>
-// PCL
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/common/io.h>
static void
#include <iostream>
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
#include <pcl/common/common.h>
int
* @author Yani Ioannou
* @date 2012-03-11
*/
+
#include <string>
-#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/common/point_operators.h>
-#include <pcl/common/io.h>
-#include <pcl/search/organized.h>
-#include <pcl/search/octree.h>
-#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
-#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/io/vtk_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/don.h>
*
*/
-
#include <iostream>
-#include <vector>
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/normal_3d.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
int
*/
#include <iostream>
-#include <vector>
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/features/normal_3d.h>
*
*/
-
#include <iostream>
-#include <vector>
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
-
int
main (int, char** argv)
{
*
*
*/
-// STL
+
#include <iostream>
-// PCL
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-#include <pcl/common/io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/rift.h>
#include <pcl/features/intensity_gradient.h>
*/
#include <iostream>
-#include <vector>
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
#include <pcl/features/3dsc.h>
-#include <pcl/features/impl/3dsc.hpp>
#include <pcl/features/normal_3d.h>
int
*
*/
-
-
#include <iostream>
-#include <vector>
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/spin_image.h>
*
*/
-// STL
#include <iostream>
-// PCL
-#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
int
indices.indices.push_back (2);
pcl::ExtractIndices<PointType> extract_indices;
- extract_indices.setIndices (boost::make_shared<const pcl::PointIndices> (indices));
+ extract_indices.setIndices (pcl::make_shared<const pcl::PointIndices> (indices));
extract_indices.setInputCloud (cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr output (new pcl::PointCloud<pcl::PointXYZ>);
extract_indices.filter (*output);
*
*/
-// STL
#include <iostream>
-#include <limits>
-// PCL
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
int
#include <iostream>
-#include <pcl/console/parse.h>
-#include <pcl/console/print.h>
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/harris_3d.h>
*
*/
-// STL
#include <iostream>
-// PCL
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-#include <pcl/common/io.h>
#include <pcl/keypoints/sift_keypoint.h>
-#include <pcl/keypoints/impl/sift_keypoint.hpp>
-#include <pcl/features/normal_3d.h>
-// #include <pcl/visualization/pcl_visualizer.h>
-
+
int
main(int, char** argv)
{
*
*/
-// STL
#include <iostream>
-// PCL
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-#include <pcl/common/io.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/features/normal_3d.h>
-// #include <pcl/visualization/pcl_visualizer.h>
/* This example shows how to estimate the SIFT points based on the
* Normal gradients i.e. curvature than using the Intensity gradient
*
*/
-// STL
#include <iostream>
-// PCL
#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-#include <pcl/common/io.h>
#include <pcl/keypoints/sift_keypoint.h>
-#include <pcl/features/normal_3d.h>
-// #include <pcl/visualization/pcl_visualizer.h>
/* This examples shows how to estimate the SIFT points based on the
* z gradient of the 3D points than using the Intensity gradient as
*/
#include <pcl/io/pcd_io.h>
-#include <pcl/console/print.h>
-
-#include <pcl/point_types.h>
-#include <pcl/PCLPointCloud2.h>
#include <pcl/outofcore/outofcore.h>
#include <pcl/outofcore/outofcore_impl.h>
-#include <pcl/outofcore/boost.h>
using namespace pcl::outofcore;
*/
#include <pcl/io/pcd_io.h>
-#include <pcl/console/print.h>
#include <pcl/outofcore/outofcore.h>
#include <pcl/outofcore/outofcore_impl.h>
-#include <pcl/outofcore/boost.h>
-
-#include<pcl/point_types.h>
-#include <pcl/PCLPointCloud2.h>
-
using namespace pcl::outofcore;
using OctreeDisk = OutofcoreOctreeBase<OutofcoreOctreeDiskContainer<pcl::PointXYZ>, pcl::PointXYZ>;
*
*/
-// Stdlib
#include <cstdlib>
-#include <cmath>
-#include <climits>
#include <thread>
#include <boost/format.hpp>
-#include <boost/filesystem.hpp>
-// PCL input/output
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/point_cloud_color_handlers.h>
-//PCL other
-#include <pcl/filters/passthrough.h>
-#include <pcl/segmentation/supervoxel_clustering.h>
-
-// The segmentation class this example is for
#include <pcl/segmentation/cpc_segmentation.h>
-// VTK
-#include <vtkImageReader2Factory.h>
-#include <vtkImageReader2.h>
-#include <vtkImageData.h>
-#include <vtkImageFlip.h>
#include <vtkPolyLine.h>
using namespace std::chrono_literals;
*
*/
-// STL
#include <iostream>
-// PCL
-#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
-#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
-
int
main (int, char **argv)
{
*
*/
-// Stdlib
#include <cstdlib>
-#include <cmath>
-#include <climits>
#include <thread>
#include <boost/format.hpp>
-
-// PCL input/output
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/point_cloud_color_handlers.h>
-
-//PCL other
-#include <pcl/filters/passthrough.h>
-#include <pcl/segmentation/supervoxel_clustering.h>
-// The segmentation class this example is for
#include <pcl/segmentation/lccp_segmentation.h>
-// VTK
-#include <vtkImageReader2Factory.h>
-#include <vtkImageReader2.h>
-#include <vtkImageData.h>
-#include <vtkImageFlip.h>
#include <vtkPolyLine.h>
using namespace std::chrono_literals;
*
*/
-// STL
#include <iostream>
-// PCL
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/region_growing.h>
-#include <pcl/kdtree/kdtree.h>
#include <pcl/common/time.h>
#include <pcl/console/parse.h>
#include <thread>
-#include <pcl/common/time.h>
#include <pcl/console/parse.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/io/png_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <pcl/surface/on_nurbs/triangulation.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/on_nurbs/fitting_curve_pdm.h>
#include <pcl/surface/on_nurbs/triangulation.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/on_nurbs/fitting_curve_2d.h>
#include <pcl/surface/on_nurbs/triangulation.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
-#include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
#include <pcl/surface/on_nurbs/triangulation.h>
using Point = pcl::PointXYZ;
#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/common/io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
#pragma once
#if defined __GNUC__
-# pragma GCC system_header
+# pragma GCC system_header
#endif
// PCL includes
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/search/search.h>
search_parameter_(0), search_radius_(0), k_(0),
fake_surface_(false)
{}
-
+
/** \brief Empty destructor */
virtual ~Feature () {}
/** \brief Empty constructor. */
FeatureFromNormals () : normals_ () {}
-
+
/** \brief Empty destructor */
virtual ~FeatureFromNormals () {}
{
k_ = 1; // Search tree is not always used here.
}
-
+
/** \brief Empty destructor */
virtual ~FeatureFromLabels () {}
* \param[in] cloud Point cloud containing the XYZ coordinates,
* \param[in] normals Point cloud containing the corresponding surface normals.
* \param[out] covariances Vector of computed covariances.
- * \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001)
+ * \param[in] epsilon Optional: Epsilon for the expected noise along the surface normal (default: 0.001)
*/
template <typename PointT, typename PointNT> inline void
computeApproximateCovariances(const pcl::PointCloud<PointT>& cloud,
shape_interp_ = interp;
}
- /** \brief Returns the transformation aligning the point cloud to the canonical coordinate system
- * \param[out] trans transformation
+ /**
+ * \brief Returns the transformation aligning the point cloud to the canonical coordinate system
*/
const Eigen::Matrix4f&
getTransform () const
/** \brief copy computed color histograms to output descriptor point cloud
* \param[in] grid_size size of the regular grid used to compute the descriptor
* \param[in] hists_size size of the color histograms
- * \param[in,out] hists color histograms, which are finalized, since they are circular
+ * \param[in,out] hists color histograms, which are finalized, since they are circular
* \param[out] output output descriptor point cloud
* \param[in,out] pos current position of output descriptor point cloud
*/
*
*/
-#ifndef PCL_FEATURES_IMPL_3DSC_HPP_
-#define PCL_FEATURES_IMPL_3DSC_HPP_
+#pragma once
-#include <cmath>
#include <pcl/features/3dsc.h>
-#include <pcl/common/utils.h>
-#include <pcl/common/geometry.h>
+
#include <pcl/common/angles.h>
+#include <pcl/common/geometry.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+#include <pcl/common/utils.h>
+
+#include <cmath>
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT> bool
#define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>;
-#endif
//extract support points for Rz radius
std::vector<int> neighbours_indices;
std::vector<float> neighbours_distances;
- int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
+ std::size_t n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
//check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis
if (n_neighbours < 6)
//copy neighbours coordinates into eigen matrix
Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
- for (int i = 0; i < n_neighbours; ++i)
+ for (std::size_t i = 0; i < n_neighbours; ++i)
{
neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
}
if (!margin_point_found)
{
//find among points with neighDistance <= marginThresh*radius
- for (int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
+ for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
{
const int& curr_neigh_idx = neighbours_indices[curr_neigh];
const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
float max_hole_prob = -std::numeric_limits<float>::max ();
//find holes
- for (auto ch = first_no_border; ch < check_margin_array_size_; ch++)
+ for (auto ch = first_no_border; ch < static_cast<std::size_t>(check_margin_array_size_); ch++)
{
if (!check_margin_array_[ch])
{
*
*/
-#ifndef PCL_FEATURES_IMPL_BOUNDARY_H_
-#define PCL_FEATURES_IMPL_BOUNDARY_H_
+#pragma once
#include <pcl/features/boundary.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
#include <cfloat>
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
#define PCL_INSTANTIATE_BoundaryEstimation(PointInT,PointNT,PointOutT) template class PCL_EXPORTS pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>;
-#endif // PCL_FEATURES_IMPL_BOUNDARY_H_
-
*
*/
+
#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_
#define PCL_FEATURES_IMPL_BRISK_2D_HPP_
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT>
-
pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::BRISK2DEstimation ()
+BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::BRISK2DEstimation ()
: rotation_invariance_enabled_ (true)
, scale_invariance_enabled_ (true)
, pattern_scale_ (1.0f)
generateKernel (r_list, n_list, 5.85f * pattern_scale_, 8.2f * pattern_scale_);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT>
-
pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::~BRISK2DEstimation ()
+BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::~BRISK2DEstimation ()
{
- if (pattern_points_) delete [] pattern_points_;
- if (short_pairs_) delete [] short_pairs_;
- if (long_pairs_) delete [] long_pairs_;
- if (scale_list_) delete [] scale_list_;
- if (size_list_) delete [] size_list_;
+ delete [] pattern_points_;
+ delete [] short_pairs_;
+ delete [] long_pairs_;
+ delete [] scale_list_;
+ delete [] size_list_;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> void
-
pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::generateKernel (
- std::vector<float> &radius_list,
+BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::generateKernel (
+ std::vector<float> &radius_list,
std::vector<int> &number_list, float d_max, float d_min,
std::vector<int> index_change)
{
{
// this is the rotation of the feature
double theta = double (rot) * 2 * M_PI / double (n_rot_);
- for (int ring = 0; ring < rings; ++ring)
+ for (int ring = 0; ring < static_cast<int>(rings); ++ring)
{
for (int num = 0; num < number_list[ring]; ++num)
{
// the actual coordinates on the circle
double alpha = double (num) * 2 * M_PI / double (number_list[ring]);
-
+
// feature rotation plus angle of the point
- pattern_iterator->x = scale_list_[scale] * radius_list[ring] * static_cast<float> (std::cos (alpha + theta));
+ pattern_iterator->x = scale_list_[scale] * radius_list[ring] * static_cast<float> (std::cos (alpha + theta));
pattern_iterator->y = scale_list_[scale] * radius_list[ring] * static_cast<float> (sin (alpha + theta));
// and the gaussian kernel sigma
if (ring == 0)
strings_ = int (std::ceil ((float (no_short_pairs_)) / 128.0)) * 4 * 4;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> inline int
-
pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::smoothedIntensity (
+BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::smoothedIntensity (
const std::vector<unsigned char> &image,
int image_width, int,
//const Stefan& integral,
const int r_y = static_cast<int> ((yf - float (y)) * 1024);
const int r_x_1 = (1024 - r_x);
const int r_y_1 = (1024 - r_y);
-
+
//+const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x + y * imagecols;
const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x + y * imagecols;
-
+
// just interpolate:
ret_val = (r_x_1 * r_y_1 * int (*ptr));
-
+
//+ptr += sizeof (PointInT);
ptr++;
ret_val += (r_x * r_y_1 * int (*ptr));
-
+
//+ptr += (imagecols * sizeof (PointInT));
ptr += imagecols;
-
+
ret_val += (r_x * r_y * int (*ptr));
-
+
//+ptr -= sizeof (PointInT);
ptr--;
-
+
ret_val += (r_x_1 * r_y * int (*ptr));
return (ret_val + 512) / 1024;
}
if (dx + dy > 2)
{
// now the calculation:
-
+
//+const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x_left + imagecols * y_top;
const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x_left + imagecols * y_top;
// first the corners:
ret_val = A * int (*ptr);
-
+
//+ptr += (dx + 1) * sizeof (PointInT);
ptr += dx + 1;
-
+
ret_val += B * int (*ptr);
-
+
//+ptr += (dy * imagecols + 1) * sizeof (PointInT);
ptr += dy * imagecols + 1;
-
+
ret_val += C * int (*ptr);
-
+
//+ptr -= (dx + 1) * sizeof (PointInT);
ptr -= dx + 1;
-
+
ret_val += D * int (*ptr);
// next the edges:
//+int* ptr_integral;// = static_cast<int*> (integral.data) + x_left + integralcols * y_top + 1;
const int* ptr_integral = static_cast<const int*> (&integral_image[0]) + x_left + integralcols * y_top + 1;
-
+
// find a simple path through the different surface corners
const int tmp1 = (*ptr_integral);
ptr_integral += dx;
}
// now the calculation:
-
+
//const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x_left + imagecols * y_top;
const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x_left + imagecols * y_top;
-
+
// first row:
ret_val = A * int (*ptr);
-
+
//+ptr += sizeof (PointInT);
ptr++;
-
+
//+const unsigned char* end1 = ptr + (dx * sizeof (PointInT));
const unsigned char* end1 = ptr + dx;
for (; ptr < end1; ptr++)
ret_val += r_y_1_i * int (*ptr);
ret_val += B * int (*ptr);
-
+
// middle ones:
//+ptr += (imagecols - dx - 1) * sizeof (PointInT);
ptr += imagecols - dx - 1;
-
+
//+const unsigned char* end_j = ptr + (dy * imagecols) * sizeof (PointInT);
const unsigned char* end_j = ptr + dy * imagecols;
-
+
//+for (; ptr < end_j; ptr += (imagecols - dx - 1) * sizeof (PointInT))
for (; ptr < end_j; ptr += imagecols - dx - 1)
{
ret_val += r_x_1_i * int (*ptr);
-
+
//+ptr += sizeof (PointInT);
ptr++;
-
+
//+const unsigned char* end2 = ptr + (dx * sizeof (PointInT));
const unsigned char* end2 = ptr + dx;
-
+
//+for (; ptr < end2; ptr += sizeof (PointInT))
for (; ptr < end2; ptr++)
ret_val += int (*ptr) * scaling;
}
// last row:
ret_val += D * int (*ptr);
-
+
//+ptr += sizeof (PointInT);
ptr++;
-
+
//+const unsigned char* end3 = ptr + (dx * sizeof (PointInT));
const unsigned char* end3 = ptr + dx;
}
-//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> bool
-
pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::RoiPredicate (
+BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::RoiPredicate (
const float min_x, const float min_y,
const float max_x, const float max_y, const KeypointT& pt)
{
return ((pt.x < min_x) || (pt.x >= max_x) || (pt.y < min_y) || (pt.y >= max_y));
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> void
-
pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::compute (
+BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::compute (
PointCloudOutT &output)
{
if (!input_cloud_->isOrganized ())
- {
+ {
PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
return;
}
// image size
- const int width = int (input_cloud_->width);
- const int height = int (input_cloud_->height);
+ const index_t width = static_cast<index_t>(input_cloud_->width);
+ const index_t height = static_cast<index_t>(input_cloud_->height);
// destination for intensity data; will be forwarded to BRISK
std::vector<unsigned char> image_data (width*height);
std::size_t ksize = keypoints_->points.size ();
std::vector<int> kscales; // remember the scale per keypoint
kscales.resize (ksize);
-
+
// initialize constants
static const float lb_scalerange = std::log2 (scalerange_);
typename std::vector<KeypointT, Eigen::aligned_allocator<KeypointT> >::iterator beginning = keypoints_->points.begin ();
std::vector<int>::iterator beginningkscales = kscales.begin ();
-
+
static const float basic_size_06 = basic_size_ * 0.6f;
unsigned int basicscale = 0;
// current integral image
std::vector<int> integral ((width+1)*(height+1), 0); // the integral image
- for (std::size_t row_index = 1; row_index < height; ++row_index)
+ for (index_t row_index = 1; row_index < height; ++row_index)
{
- for (std::size_t col_index = 1; col_index < width; ++col_index)
+ for (index_t col_index = 1; col_index < width; ++col_index)
{
const std::size_t index = row_index*width+col_index;
const std::size_t index2 = (row_index)*(width+1)+(col_index);
// now iterate through all the pairings
UINT32_ALIAS* ptr2 = reinterpret_cast<UINT32_ALIAS*> (ptr);
const BriskShortPair* max = short_pairs_ + no_short_pairs_;
-
+
for (BriskShortPair* iter = short_pairs_; iter < max; ++iter)
{
t1 = *(values + iter->i);
t2 = *(values + iter->j);
-
+
if (t1 > t2)
*ptr2 |= ((1) << shifter);
}
//ptr += strings_;
-
+
//// Account for the scale + orientation;
//ptr += sizeof (output.points[0].scale);
//ptr += sizeof (output.points[0].orientation);
delete [] values;
}
+} // namespace pcl
#endif //#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_
{
PointOutT p;
// No need to calculate feature for identity pair (i, j) as they aren't used in future calculations
- if (i != j)
+ // @TODO: resolve issue with comparison in a better manner
+ if (static_cast<std::size_t>(i) != j)
{
if (
pcl::computeCPPFPairFeature (input_->points[i].getVector4fMap (),
#include <pcl/search/pcl_search.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
inline void
-pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
- const Eigen::Vector4f &point,
- Eigen::Vector4f &plane_parameters, float &curvature)
+solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
+ const Eigen::Vector4f &point,
+ Eigen::Vector4f &plane_parameters, float &curvature)
{
solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);
plane_parameters[3] = -1 * plane_parameters.dot (point);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
inline void
-pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
- float &nx, float &ny, float &nz, float &curvature)
+solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
+ float &nx, float &ny, float &nz, float &curvature)
{
// Avoid getting hung on Eigen's optimizers
// for (int i = 0; i < 9; ++i)
curvature = 0;
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT> bool
-
pcl::Feature<PointInT, PointOutT>::initCompute ()
+Feature<PointInT, PointOutT>::initCompute ()
{
if (!PCLBase<PointInT>::initCompute ())
{
else
tree_.reset (new pcl::search::KdTree<PointInT> (false));
}
-
+
if (tree_->getInputCloud () != surface_) // Make sure the tree searches the surface
- tree_->setInputCloud (surface_);
+ tree_->setInputCloud (surface_);
// Do a fast check to see if the search parameters are well defined
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT> bool
-
pcl::Feature<PointInT, PointOutT>::deinitCompute ()
+Feature<PointInT, PointOutT>::deinitCompute ()
{
// Reset the surface
if (fake_surface_)
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT> void
-
pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output)
+Feature<PointInT, PointOutT>::compute (PointCloudOut &output)
{
if (!initCompute ())
{
deinitCompute ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointNT, typename PointOutT> bool
-
pcl::FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ()
+FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ()
{
if (!Feature<PointInT, PointOutT>::initCompute ())
{
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointLT, typename PointOutT> bool
-
pcl::FeatureFromLabels<PointInT, PointLT, PointOutT>::initCompute ()
+FeatureFromLabels<PointInT, PointLT, PointOutT>::initCompute ()
{
if (!Feature<PointInT, PointOutT>::initCompute ())
{
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointRFT> bool
-
pcl::FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (const std::size_t& indices_size,
- const LRFEstimationPtr& lrf_estimation)
+FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (const std::size_t& indices_size,
+ const LRFEstimationPtr& lrf_estimation)
{
if (frames_never_defined_)
frames_.reset ();
return (true);
}
+} // namespace pcl
+
#endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_
}
if (sampled_tree_->getInputCloud () != sampled_surface_) // Make sure the tree searches the sampled surface
- sampled_tree_->setInputCloud (sampled_surface_);
+ sampled_tree_->setInputCloud (sampled_surface_);
return (true);
}
const std::size_t n_normal_neighbours =
this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
- if (n_normal_neighbours < min_neighbors_for_normal_axis_)
+ if (n_normal_neighbours < static_cast<std::size_t>(min_neighbors_for_normal_axis_))
{
if (!pcl::isFinite ((*normals_)[index]))
{
const std::size_t n_tangent_neighbours =
sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances);
- if (n_tangent_neighbours < min_neighbors_for_tangent_axis_)
+ if (n_tangent_neighbours < static_cast<std::size_t>(min_neighbors_for_tangent_axis_))
{
//set X axis as a random axis
x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal);
*
*/
-#ifndef PCL_FEATURES_IMPL_FPFH_H_
-#define PCL_FEATURES_IMPL_FPFH_H_
+#pragma once
#include <pcl/features/fpfh.h>
+
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/features/pfh_tools.h>
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointInT, typename PointNT, typename PointOutT> void
+template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (
const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
int p_idx, int row, const std::vector<int> &indices,
weight = 1.0f / dists[idx];
// Weight the SPFH of the query point with the SPFH of its neighbors
- for (std::size_t f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
+ for (Eigen::MatrixXf::Index f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
{
val_f1 = hist_f1 (indices[idx], f1_i) * weight;
sum_f1 += val_f1;
fpfh_histogram[f1_i] += val_f1;
}
- for (std::size_t f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
+ for (Eigen::MatrixXf::Index f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
{
val_f2 = hist_f2 (indices[idx], f2_i) * weight;
sum_f2 += val_f2;
fpfh_histogram[f2_i + nr_bins_f1] += val_f2;
}
- for (std::size_t f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
+ for (Eigen::MatrixXf::Index f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
{
val_f3 = hist_f3 (indices[idx], f3_i) * weight;
sum_f3 += val_f3;
continue;
}
- // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
+ // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (auto &nn_index : nn_indices)
nn_index = spfh_hist_lookup[nn_index];
continue;
}
- // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
+ // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (auto &nn_index : nn_indices)
nn_index = spfh_hist_lookup[nn_index];
#define PCL_INSTANTIATE_FPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimation<T,NT,OutT>;
-#endif // PCL_FEATURES_IMPL_FPFH_H_
-
*
*/
-#ifndef PCL_FEATURES_IMPL_FPFH_OMP_H_
-#define PCL_FEATURES_IMPL_FPFH_OMP_H_
+#pragma once
+
+#include <pcl/features/fpfh_omp.h>
+
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <numeric>
-#include <pcl/features/fpfh_omp.h>
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT> void
// Compute SPFH signatures for every point that needs them
-#ifdef _OPENMP
-#pragma omp parallel for shared (spfh_hist_lookup) private (nn_indices, nn_dists) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(spfh_hist_lookup, spfh_indices_vec) \
+ private(nn_indices, nn_dists) \
+ num_threads(threads_)
for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t> (spfh_indices_vec.size ()); ++i)
{
// Get the next point index
nn_dists.clear();
// Iterate over the entire index vector
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(nr_bins, output, spfh_hist_lookup) \
+ private(nn_dists, nn_indices) \
+ num_threads(threads_)
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
{
// Find the indices of point idx's neighbors...
#define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP<T,NT,OutT>;
-#endif // PCL_FEATURES_IMPL_FPFH_OMP_H_
-
* $Id: feature.h 2784 2011-10-15 22:05:38Z aichim $
*/
+
#ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_
#define PCL_INTEGRAL_IMAGE2D_IMPL_H_
#include <cstddef>
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename DataType, unsigned Dimension> void
-pcl::IntegralImage2D<DataType, Dimension>::setSecondOrderComputation (bool compute_second_order_integral_images)
+IntegralImage2D<DataType, Dimension>::setSecondOrderComputation (bool compute_second_order_integral_images)
{
compute_second_order_integral_images_ = compute_second_order_integral_images;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType, unsigned Dimension> void
-pcl::IntegralImage2D<DataType, Dimension>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
+IntegralImage2D<DataType, Dimension>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
{
if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
{
computeIntegralImages (data, row_stride, element_stride);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
-pcl::IntegralImage2D<DataType, Dimension>::getFirstOrderSum (
+IntegralImage2D<DataType, Dimension>::getFirstOrderSum (
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
-pcl::IntegralImage2D<DataType, Dimension>::getSecondOrderSum (
+IntegralImage2D<DataType, Dimension>::getSecondOrderSum (
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType, unsigned Dimension> unsigned
-pcl::IntegralImage2D<DataType, Dimension>::getFiniteElementsCount (
+IntegralImage2D<DataType, Dimension>::getFiniteElementsCount (
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
-pcl::IntegralImage2D<DataType, Dimension>::getFirstOrderSumSE (
+IntegralImage2D<DataType, Dimension>::getFirstOrderSumSE (
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
-pcl::IntegralImage2D<DataType, Dimension>::getSecondOrderSumSE (
+IntegralImage2D<DataType, Dimension>::getSecondOrderSumSE (
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType, unsigned Dimension> unsigned
-pcl::IntegralImage2D<DataType, Dimension>::getFiniteElementsCountSE (
+IntegralImage2D<DataType, Dimension>::getFiniteElementsCountSE (
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType, unsigned Dimension> void
-pcl::IntegralImage2D<DataType, Dimension>::computeIntegralImages (
+IntegralImage2D<DataType, Dimension>::computeIntegralImages (
const DataType *data, unsigned row_stride, unsigned element_stride)
{
ElementType* previous_row = &first_order_integral_image_[0];
}
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename DataType> void
-pcl::IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
+IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
{
if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
{
computeIntegralImages (data, row_stride, element_stride);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
-pcl::IntegralImage2D<DataType, 1>::getFirstOrderSum (
+IntegralImage2D<DataType, 1>::getFirstOrderSum (
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
-pcl::IntegralImage2D<DataType, 1>::getSecondOrderSum (
+IntegralImage2D<DataType, 1>::getSecondOrderSum (
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType> unsigned
-pcl::IntegralImage2D<DataType, 1>::getFiniteElementsCount (
+IntegralImage2D<DataType, 1>::getFiniteElementsCount (
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
-pcl::IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
+IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
-pcl::IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
+IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType> unsigned
-pcl::IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
+IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
{
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename DataType> void
-pcl::IntegralImage2D<DataType, 1>::computeIntegralImages (
+IntegralImage2D<DataType, 1>::computeIntegralImages (
const DataType *data, unsigned row_stride, unsigned element_stride)
{
ElementType* previous_row = &first_order_integral_image_[0];
}
}
}
+
+} // namespace pcl
+
#endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_
*
*/
-#ifndef PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_
-#define PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_
+#pragma once
#include <pcl/features/intensity_gradient.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
pcl::IntensityGradientEstimation <PointInT, PointNT, PointOutT, IntensitySelectorT>::computePointIntensityGradient (
A (2, 0) = A (0, 2);
A (2, 1) = A (1, 2);
-//*
- Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b);
-/*/
+// Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b);
Eigen::Vector3f eigen_values;
Eigen::Matrix3f eigen_vectors;
// x [2] = b.dot (A.col (2));
// if (A.col (2).squaredNorm () != 0)
// x[2] /= A.col (2).squaredNorm ();
- // Fit a hyperplane to the data
-
-//*/
+// // Fit a hyperplane to the data
+//
// std::cout << A << "\n*\n" << bb << "\n=\n" << x << "\nvs.\n" << x2 << "\n\n";
// std::cout << A * x << "\nvs.\n" << A * x2 << "\n\n------\n";
// Project the gradient vector, x, onto the tangent plane
// If the data is dense, we don't need to check for NaN
if (surface_->is_dense)
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(nn_indices, nn_dists) \
+ num_threads(threads_)
// Iterating over the entire index vector
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
{
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(nn_indices, nn_dists) \
+ num_threads(threads_)
// Iterating over the entire index vector
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
{
#define PCL_INSTANTIATE_IntensityGradientEstimation(InT,NT,OutT) template class PCL_EXPORTS pcl::IntensityGradientEstimation<InT,NT,OutT>;
-#endif // PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
if (input_->is_dense)
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ firstprivate(nn_indices, nn_dists) \
+ num_threads(threads_)
// Iterating over the entire index vector
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
{
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ firstprivate(nn_indices, nn_dists) \
+ num_threads(threads_)
// Iterating over the entire index vector
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
{
*
*/
-#ifndef PCL_FEATURES_IMPL_PFH_H_
-#define PCL_FEATURES_IMPL_PFH_H_
+#pragma once
#include <pcl/features/pfh.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
#define PCL_INSTANTIATE_PFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHEstimation<T,NT,OutT>;
-#endif // PCL_FEATURES_IMPL_PFH_H_
-
*
*/
-#ifndef PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_
-#define PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_
+#pragma once
#include <pcl/features/principal_curvatures.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computePointPrincipalCurvatures (
#define PCL_INSTANTIATE_PrincipalCurvaturesEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PrincipalCurvaturesEstimation<T,NT,OutT>;
-#endif // PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_
}
tree_->setSortedResults (true);
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ num_threads(threads_)
for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t> (indices_->size ()); ++i)
{
// point result
*
*/
-#ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_
-#define PCL_FEATURES_IMPL_SHOT_OMP_H_
+#pragma once
#include <pcl/features/shot_omp.h>
+
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/common/time.h>
#include <pcl/features/shot_lrf_omp.h>
+
template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
pcl::SHOTEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::initCompute ()
{
output.is_dense = true;
// Iterating over the entire index vector
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ num_threads(threads_)
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
{
output.is_dense = true;
// Iterating over the entire index vector
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ num_threads(threads_)
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
{
Eigen::VectorXf shot;
#define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationOMP<T,NT,OutT,RFT>;
#define PCL_INSTANTIATE_SHOTColorEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimationOMP<T,NT,OutT,RFT>;
-#endif // PCL_FEATURES_IMPL_SHOT_OMP_H_
*
*/
-#ifndef PCL_FEATURES_IMPL_USC_HPP_
-#define PCL_FEATURES_IMPL_USC_HPP_
+#pragma once
#include <pcl/features/usc.h>
#include <pcl/features/shot_lrf.h>
-#include <pcl/common/geometry.h>
#include <pcl/common/angles.h>
+#include <pcl/common/geometry.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/common/utils.h>
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename PointRFT> bool
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::initCompute ()
#define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT,RFT>;
-#endif
#pragma once
+#include <pcl/memory.h>
+
#include <vector>
-#include <boost/shared_ptr.hpp>
namespace pcl
{
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <vector>
#include <cmath>
#include <pcl/features/feature.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/PointIndices.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/features/eigen.h>
#include <pcl/common/common_headers.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/features/feature.h>
#include <pcl/common/centroid.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/PolygonMesh.h>
#include <pcl/features/feature.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/features/feature.h>
int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false);
template <typename PointInT, typename PointNT, typename PointOutT>
- PCL_DEPRECATED("use computeRSD() overload that takes input point clouds by const reference")
+ PCL_DEPRECATED(1, 12, "use computeRSD() overload that takes input point clouds by const reference")
Eigen::MatrixXf
computeRSD (typename pcl::PointCloud<PointInT>::ConstPtr &surface, typename pcl::PointCloud<PointNT>::ConstPtr &normals,
const std::vector<int> &indices, double max_dist,
int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false);
template <typename PointNT, typename PointOutT>
- PCL_DEPRECATED("use computeRSD() overload that takes input point cloud by const reference")
+ PCL_DEPRECATED(1, 12, "use computeRSD() overload that takes input point cloud by const reference")
Eigen::MatrixXf
computeRSD (typename pcl::PointCloud<PointNT>::ConstPtr &normals,
const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,
Narf::extractForInterestPoints (const RangeImage& range_image, const PointCloud<InterestPoint>& interest_points,
int descriptor_size, float support_size, bool rotation_invariant, std::vector<Narf*>& feature_list)
{
- # pragma omp parallel for num_threads(max_no_of_threads) default(shared) schedule(dynamic, 10)
+#pragma omp parallel for \
+ default(none) \
+ shared(descriptor_size, feature_list, interest_points, range_image, rotation_invariant, support_size) \
+ schedule(dynamic, 10) \
+ num_threads(max_no_of_threads)
//!!! nizar 20110408 : for OpenMP sake on MSVC this must be kept signed
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t>(interest_points.points.size ()); ++idx)
{
#include <pcl/features/eigen.h>
#include <pcl/features/range_image_border_extractor.h>
-namespace pcl
+namespace pcl
{
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::setRangeImage (const RangeImage* range_image)
{
clearData ();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::clearData ()
{
//std::cout << PVARC(range_image_size_during_extraction_)<<PVARN((void*)this);
delete border_directions_[i];
}
delete[] surface_structure_; surface_structure_ = nullptr;
- delete border_descriptions_; border_descriptions_ = nullptr;
delete[] shadow_border_informations_; shadow_border_informations_ = nullptr;
delete[] border_directions_; border_directions_ = nullptr;
-
+ delete border_descriptions_; border_descriptions_ = nullptr;
+
delete[] surface_change_scores_; surface_change_scores_ = nullptr;
delete[] surface_change_directions_; surface_change_directions_ = nullptr;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::extractLocalSurfaceStructure ()
{
if (surface_structure_ != nullptr)
return;
//std::cerr << __PRETTY_FUNCTION__<<" called (this="<<(void*)this<<").\n";
//MEASURE_FUNCTION_TIME;
-
- const auto width = range_image_->width,
- height = range_image_->height;
+
+ const auto width = range_image_->width;
+ const auto height = range_image_->height;
range_image_size_during_extraction_ = width*height;
const auto array_size = range_image_size_during_extraction_;
surface_structure_ = new LocalSurface*[array_size];
const auto sqrt_neighbors = parameters_.pixel_radius_plane_extraction/step_size + 1;
const auto no_of_nearest_neighbors = sqrt_neighbors * sqrt_neighbors;
-# pragma omp parallel for num_threads(parameters_.max_no_of_threads) default(shared) schedule(dynamic, 10)
- for (int y=0; y<height; ++y)
+ // iteration_type should be at least as big as unsigned int (decltype of height)
+ // But OpenMP requires signed size. Here we choose the minimum size that fits the bill
+ using iteration_type = std::conditional_t<sizeof(int) == sizeof(long int), long long int, long int>;
+
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ schedule(dynamic, 10) \
+ num_threads(parameters_.max_no_of_threads)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(height, no_of_nearest_neighbors, step_size, width) \
+ schedule(dynamic, 10) \
+ num_threads(parameters_.max_no_of_threads)
+#endif
+ for (iteration_type y=0; y<height; ++y)
{
- for (int x=0; x<width; ++x)
+ for (unsigned int x=0; x<width; ++x)
{
std::size_t index = y*width + x;
LocalSurface*& local_surface = surface_structure_[index];
delete local_surface;
local_surface = nullptr;
}
-
+
//std::cout << x<<","<<y<<": ("<<local_surface->normal_no_jumps[0]<<","<<local_surface->normal_no_jumps[1]<<","<<local_surface->normal_no_jumps[2]<<")\n";
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::extractBorderScoreImages ()
{
if (!border_scores_left_.empty())
return;
-
+
extractLocalSurfaceStructure();
-
+
//MEASURE_FUNCTION_TIME;
-
+
int width = range_image_->width,
height = range_image_->height,
size = width*height;
border_scores_right_.resize (size);
border_scores_top_.resize (size);
border_scores_bottom_ .resize (size);
-
-# pragma omp parallel for num_threads(parameters_.max_no_of_threads) default(shared) schedule(dynamic, 10)
+
+#pragma omp parallel for \
+ default(none) \
+ shared(height, width) \
+ schedule(dynamic, 10) \
+ num_threads(parameters_.max_no_of_threads)
for (int y=0; y<height; ++y) {
for (int x=0; x<width; ++x) {
int index = y*width + x;
float& left=border_scores_left_[index]; float& right=border_scores_right_[index];
- float& top=border_scores_top_[index]; float& bottom=border_scores_bottom_[index];
+ float& top=border_scores_top_[index]; float& bottom=border_scores_bottom_[index];
LocalSurface* local_surface_ptr = surface_structure_[index];
if (local_surface_ptr==nullptr)
{
left=right=top=bottom = 0.0f;
continue;
}
-
+
left = getNeighborDistanceChangeScore(*local_surface_ptr, x, y, -1, 0, parameters_.pixel_radius_borders);
right = getNeighborDistanceChangeScore(*local_surface_ptr, x, y, 1, 0, parameters_.pixel_radius_borders);
top = getNeighborDistanceChangeScore(*local_surface_ptr, x, y, 0, -1, parameters_.pixel_radius_borders);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-float*
+float*
RangeImageBorderExtractor::updatedScoresAccordingToNeighborValues (const float* border_scores) const
{
float* new_scores = new float[range_image_->width*range_image_->height];
float* new_scores_ptr = new_scores;
- for (int y=0; y < static_cast<int> (range_image_->height); ++y)
- for (int x=0; x < static_cast<int> (range_image_->width); ++x)
+ for (int y=0; y < static_cast<int> (range_image_->height); ++y)
+ for (int x=0; x < static_cast<int> (range_image_->width); ++x)
*(new_scores_ptr++) = updatedScoreAccordingToNeighborValues(x, y, border_scores);
return (new_scores);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::updateScoresAccordingToNeighborValues ()
{
extractBorderScoreImages();
-
+
//MEASURE_FUNCTION_TIME;
-
+
border_scores_left_ = updatedScoresAccordingToNeighborValues(border_scores_left_);
border_scores_right_ = updatedScoresAccordingToNeighborValues(border_scores_right_);
border_scores_top_ = updatedScoresAccordingToNeighborValues(border_scores_top_);
border_scores_bottom_ = updatedScoresAccordingToNeighborValues(border_scores_bottom_);
}
-
+
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::findAndEvaluateShadowBorders ()
{
if (shadow_border_informations_ != nullptr)
return;
-
+
if (border_scores_left_.empty ())
{
std::cerr << __PRETTY_FUNCTION__<<": border score images not available!\n";
}
-
+
//MEASURE_FUNCTION_TIME;
-
+
int width = range_image_->width,
height = range_image_->height;
shadow_border_informations_ = new ShadowBorderIndices*[width*height];
- for (int y = 0; y < static_cast<int> (height); ++y)
+ for (int y = 0; y < static_cast<int> (height); ++y)
{
- for (int x = 0; x < static_cast<int> (width); ++x)
+ for (int x = 0; x < static_cast<int> (width); ++x)
{
int index = y*width+x;
ShadowBorderIndices*& shadow_border_indices = shadow_border_informations_[index];
shadow_border_indices = nullptr;
int shadow_border_idx;
-
+
if (changeScoreAccordingToShadowBorderValue(x, y, -1, 0, border_scores_left_.data (), border_scores_right_.data (), shadow_border_idx))
{
shadow_border_indices = (shadow_border_indices==nullptr ? new ShadowBorderIndices : shadow_border_indices);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-float*
+float*
RangeImageBorderExtractor::getAnglesImageForBorderDirections ()
{
calculateBorderDirections();
-
+
int width = range_image_->width,
height = range_image_->height,
array_size = width*height;
float* angles_image = new float[array_size];
-
+
for (int y=0; y<height; ++y)
{
for (int x=0; x<width; ++x)
continue;
const Eigen::Vector3f& border_direction = *border_direction_ptr;
const PointWithRange& point = range_image_->getPoint(index);
-
+
float border_direction_in_image_x, border_direction_in_image_y;
float tmp_factor = point.range*range_image_->getAngularResolution();
range_image_->getImagePoint(point.x+tmp_factor*border_direction[0], point.y+tmp_factor*border_direction[1], point.z+tmp_factor*border_direction[2],
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-float*
+float*
RangeImageBorderExtractor::getAnglesImageForSurfaceChangeDirections ()
{
//MEASURE_FUNCTION_TIME;
-
+
calculateSurfaceChanges();
-
+
int width = range_image_->width,
height = range_image_->height,
array_size = width*height;
float* angles_image = new float[array_size];
-
+
for (int y=0; y<height; ++y)
{
for (int x=0; x<width; ++x)
continue;
const Eigen::Vector3f& direction = surface_change_directions_[index];
const PointWithRange& point = range_image_->getPoint(index);
-
+
float border_direction_in_image_x, border_direction_in_image_y;
float tmp_factor = point.range*range_image_->getAngularResolution();
range_image_->getImagePoint(point.x+tmp_factor*direction[0], point.y+tmp_factor*direction[1], point.z+tmp_factor*direction[2],
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::classifyBorders ()
{
if (border_descriptions_ != nullptr)
return;
-
+
// Get local plane approximations
extractLocalSurfaceStructure();
-
+
// Get scores for every point, describing how probable a border in that direction is
extractBorderScoreImages();
-
+
// Propagate values to neighboring pixels
updateScoresAccordingToNeighborValues();
-
+
// Change border score according to the existence of a shadow border
findAndEvaluateShadowBorders();
-
+
int width = range_image_->width,
height = range_image_->height,
size = width*height;
-
+
BorderDescription initial_border_description;
initial_border_description.traits = 0;
border_descriptions_ = new PointCloudOut;
border_descriptions_->height = height;
border_descriptions_->is_dense = true;
border_descriptions_->points.resize(size, initial_border_description);
-
- for (int y = 0; y < static_cast<int> (height); ++y)
+
+ for (int y = 0; y < static_cast<int> (height); ++y)
{
- for (int x = 0; x < static_cast<int> (width); ++x)
+ for (int x = 0; x < static_cast<int> (width); ++x)
{
int index = y*width+x;
BorderDescription& border_description = border_descriptions_->points[index];
border_description.x = x;
border_description.y = y;
BorderTraits& border_traits = border_description.traits;
-
+
ShadowBorderIndices* shadow_border_indices = shadow_border_informations_[index];
if (shadow_border_indices == nullptr)
continue;
-
+
int shadow_border_index = shadow_border_indices->left;
if (shadow_border_index >= 0 && checkIfMaximum(x, y, -1, 0, border_scores_left_.data (), shadow_border_index))
{
veil_point[BORDER_TRAIT__VEIL_POINT] = veil_point[BORDER_TRAIT__VEIL_POINT_RIGHT] = true;
}
}
-
+
shadow_border_index = shadow_border_indices->right;
if (shadow_border_index >= 0 && checkIfMaximum(x, y, 1, 0, border_scores_right_.data (), shadow_border_index))
{
veil_point[BORDER_TRAIT__VEIL_POINT] = veil_point[BORDER_TRAIT__VEIL_POINT_LEFT] = true;
}
}
-
+
shadow_border_index = shadow_border_indices->top;
if (shadow_border_index >= 0 && checkIfMaximum(x, y, 0, -1, border_scores_top_.data (), shadow_border_index))
{
veil_point[BORDER_TRAIT__VEIL_POINT] = veil_point[BORDER_TRAIT__VEIL_POINT_BOTTOM] = true;
}
}
-
+
shadow_border_index = shadow_border_indices->bottom;
if (shadow_border_index >= 0 && checkIfMaximum(x, y, 0, 1, border_scores_bottom_.data (), shadow_border_index))
{
veil_point[BORDER_TRAIT__VEIL_POINT] = veil_point[BORDER_TRAIT__VEIL_POINT_TOP] = true;
}
}
-
+
//if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER])
//{
//border_points.push_back(&border_description);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::calculateBorderDirections ()
{
if (border_directions_!=nullptr)
return;
classifyBorders();
-
+
//MEASURE_FUNCTION_TIME;
-
+
int width = range_image_->width,
height = range_image_->height,
size = width*height;
calculateBorderDirection(x, y);
}
}
-
+
Eigen::Vector3f** average_border_directions = new Eigen::Vector3f*[size];
int radius = parameters_.pixel_radius_border_direction;
int minimum_weight = radius+1;
const Eigen::Vector3f* neighbor_border_direction = border_directions_[index2];
if (neighbor_border_direction==nullptr || index2==index)
continue;
-
+
// Opposite directions?
float cos_angle = neighbor_border_direction->dot(*border_direction);
if (cos_angle<min_cos_angle)
}
//else
//std::cout << "No reject\n";
-
+
// Border in between?
float border_between_points_score = getNeighborDistanceChangeScore(*surface_structure_[index], x, y, x2-x, y2-y, 1);
if (std::abs(border_between_points_score) >= 0.95f*parameters_.minimum_border_probability)
continue;
-
+
*average_border_direction += *neighbor_border_direction;
weight_sum += 1.0f;
}
average_border_direction->normalize();
}
}
-
+
for (int i=0; i<size; ++i)
delete border_directions_[i];
delete[] border_directions_;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::calculateSurfaceChanges ()
{
if (surface_change_scores_!=nullptr)
return;
-
+
calculateBorderDirections();
-
+
//MEASURE_FUNCTION_TIME;
-
+
int width = range_image_->width,
height = range_image_->height,
size = width*height;
surface_change_scores_ = new float[size];
surface_change_directions_ = new Eigen::Vector3f[size];
-# pragma omp parallel for num_threads(parameters_.max_no_of_threads) default(shared) schedule(dynamic, 10)
+#pragma omp parallel for \
+ default(none) \
+ shared(height, width) \
+ schedule(dynamic, 10) \
+ num_threads(parameters_.max_no_of_threads)
for (int y=0; y<height; ++y)
{
for (int x=0; x<width; ++x)
surface_change_score = 0.0f;
Eigen::Vector3f& surface_change_direction = surface_change_directions_[index];
surface_change_direction.setZero();
-
+
const BorderTraits& border_traits = border_descriptions_->points[index].traits;
if (border_traits[BORDER_TRAIT__VEIL_POINT] || border_traits[BORDER_TRAIT__SHADOW_BORDER])
continue;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::blurSurfaceChanges ()
{
int blur_radius = 1;
-
+
const RangeImage& range_image = *range_image_;
-
+
Eigen::Vector3f* blurred_directions = new Eigen::Vector3f[range_image.width*range_image.height];
float* blurred_scores = new float[range_image.width*range_image.height];
for (int y=0; y<int(range_image.height); ++y)
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::computeFeature (PointCloudOut& output)
{
output.points.clear();
-
+
if (indices_)
{
std::cerr << __PRETTY_FUNCTION__
output.points.clear ();
return;
}
-
+
if (range_image_==nullptr)
{
std::cerr << __PRETTY_FUNCTION__
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
+void
RangeImageBorderExtractor::compute (PointCloudOut& output)
{
computeFeature (output);
set(srcs
src/conditional_removal.cpp
+ src/convolution.cpp
src/crop_box.cpp
src/extract_indices.cpp
src/filter.cpp
// Marking all Boost headers as system headers to remove warnings
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/make_shared.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/mpl/size.hpp>
#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include "clipper3D.h"
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <vector>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/common/eigen.h>
#include <pcl/filters/filter.h>
#pragma once
#include <pcl/common/eigen.h>
-#include <pcl/common/point_operators.h>
#include <pcl/point_cloud.h>
#include <pcl/exceptions.h>
#include <pcl/pcl_base.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/filters/filter_indices.h>
* \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false).
*/
CropBox (bool extract_removed_indices = false) :
- FilterIndices<PointT>::FilterIndices (extract_removed_indices),
+ FilterIndices<PointT> (extract_removed_indices),
min_pt_ (Eigen::Vector4f (-1, -1, -1, 1)),
max_pt_ (Eigen::Vector4f (1, 1, 1, 1)),
rotation_ (Eigen::Vector3f::Zero ()),
using FilterIndices<PointT>::extract_removed_indices_;
using FilterIndices<PointT>::removed_indices_;
- /** \brief Sample of point indices into a separate PointCloud
- * \param[out] output the resultant point cloud
- */
- void
- applyFilter (PointCloud &output) override;
-
/** \brief Sample of point indices
* \param[out] indices the resultant point cloud indices
*/
class Filter : public PCLBase<PointT>
{
public:
- using PCLBase<PointT>::indices_;
- using PCLBase<PointT>::input_;
-
using Ptr = shared_ptr<Filter<PointT> >;
using ConstPtr = shared_ptr<const Filter<PointT> >;
{
}
- /** \brief Empty destructor */
- ~Filter () {}
-
/** \brief Get the point indices being removed */
inline IndicesConstPtr const
getRemovedIndices () const
protected:
+ using PCLBase<PointT>::indices_;
+ using PCLBase<PointT>::input_;
+
using PCLBase<PointT>::initCompute;
using PCLBase<PointT>::deinitCompute;
{
}
- /** \brief Empty destructor */
- ~Filter () {}
-
/** \brief Get the point indices being removed */
inline IndicesConstPtr const
getRemovedIndices () const
* \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false).
*/
FilterIndices (bool extract_removed_indices = false) :
- negative_ (false),
- keep_organized_ (false),
+ Filter<PointT> (extract_removed_indices),
+ negative_ (false),
+ keep_organized_ (false),
user_filter_value_ (std::numeric_limits<float>::quiet_NaN ())
- {
- extract_removed_indices_ = extract_removed_indices;
- }
-
- /** \brief Empty virtual destructor. */
-
- ~FilterIndices ()
{
}
- inline void
- filter (PointCloud &output)
- {
- pcl::Filter<PointT>::filter (output);
- }
+ using Filter<PointT>::filter;
/** \brief Calls the filtering method and returns the filtered point cloud indices.
* \param[out] indices the resultant filtered point cloud indices
*/
- inline void
+ void
filter (std::vector<int> &indices)
{
if (!initCompute ())
using Filter<PointT>::initCompute;
using Filter<PointT>::deinitCompute;
+ using Filter<PointT>::input_;
+ using Filter<PointT>::removed_indices_;
/** \brief False = normal filter behavior (default), true = inverted behavior. */
bool negative_;
/** \brief Abstract filter method for point cloud. */
void
- applyFilter (PointCloud &output) override = 0;
+ applyFilter (PointCloud &output) override;
};
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
* \param[in] extract_removed_indices Set to true if you want to extract the indices of points being removed (default = false).
*/
FilterIndices (bool extract_removed_indices = false) :
+ Filter<PCLPointCloud2> (extract_removed_indices),
negative_ (false),
keep_organized_ (false),
user_filter_value_ (std::numeric_limits<float>::quiet_NaN ())
- {
- extract_removed_indices_ = extract_removed_indices;
- }
-
- /** \brief Empty virtual destructor. */
-
- ~FilterIndices ()
{
}
- virtual void
- filter (PCLPointCloud2 &output)
- {
- pcl::Filter<PCLPointCloud2>::filter (output);
- }
+ using Filter<PCLPointCloud2>::filter;
/** \brief Calls the filtering method and returns the filtered point cloud indices.
* \param[out] indices the resultant filtered point cloud indices
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter_indices.h>
using Filter<PointT>::getClassName;
FrustumCulling (bool extract_removed_indices = false)
- : FilterIndices<PointT>::FilterIndices (extract_removed_indices)
+ : FilterIndices<PointT> (extract_removed_indices)
, camera_pose_ (Eigen::Matrix4f::Identity ())
, hfov_ (60.0f)
, vfov_ (60.0f)
using FilterIndices<PointT>::extract_removed_indices_;
using FilterIndices<PointT>::removed_indices_;
- /** \brief Sample of point indices into a separate PointCloud
- * \param[out] output the resultant point cloud
- */
- void
- applyFilter (PointCloud &output) override;
-
/** \brief Sample of point indices
* \param[out] indices the resultant point cloud indices
*/
template <typename PointT>
pcl::FieldComparison<PointT>::~FieldComparison ()
{
- if (point_data_ != nullptr)
- {
- delete point_data_;
- point_data_ = nullptr;
- }
+ delete point_data_;
}
//////////////////////////////////////////////////////////////////////////
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
+ * Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
*
*/
-#ifndef PCL_FILTERS_CONVOLUTION_IMPL_HPP
-#define PCL_FILTERS_CONVOLUTION_IMPL_HPP
+#pragma once
#include <pcl/pcl_config.h>
#include <pcl/common/distances.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
+
+namespace pcl
+{
+namespace filters
+{
template <typename PointIn, typename PointOut>
-
pcl::filters::Convolution<PointIn, PointOut>::Convolution ()
+Convolution<PointIn, PointOut>::Convolution ()
: borders_policy_ (BORDERS_POLICY_IGNORE)
, distance_threshold_ (std::numeric_limits<float>::infinity ())
, input_ ()
{}
template <typename PointIn, typename PointOut> void
-
pcl::filters::Convolution<PointIn, PointOut>::initCompute (PointCloud<PointOut>& output)
+Convolution<PointIn, PointOut>::initCompute (PointCloud<PointOut>& output)
{
if (borders_policy_ != BORDERS_POLICY_IGNORE &&
borders_policy_ != BORDERS_POLICY_MIRROR &&
}
template <typename PointIn, typename PointOut> inline void
-
pcl::filters::Convolution<PointIn, PointOut>::convolveRows (PointCloudOut& output)
+Convolution<PointIn, PointOut>::convolveRows (PointCloudOut& output)
{
try
{
}
template <typename PointIn, typename PointOut> inline void
-
pcl::filters::Convolution<PointIn, PointOut>::convolveCols (PointCloudOut& output)
+Convolution<PointIn, PointOut>::convolveCols (PointCloudOut& output)
{
try
{
}
template <typename PointIn, typename PointOut> inline void
-
pcl::filters::Convolution<PointIn, PointOut>::convolve (const Eigen::ArrayXf& h_kernel,
- const Eigen::ArrayXf& v_kernel,
-
PointCloud<PointOut>& output)
+Convolution<PointIn, PointOut>::convolve (const Eigen::ArrayXf& h_kernel,
+ const Eigen::ArrayXf& v_kernel,
+ PointCloud<PointOut>& output)
{
try
{
}
template <typename PointIn, typename PointOut> inline void
-
pcl::filters::Convolution<PointIn, PointOut>::convolve (PointCloud<PointOut>& output)
+Convolution<PointIn, PointOut>::convolve (PointCloud<PointOut>& output)
{
try
{
}
template <typename PointIn, typename PointOut> inline PointOut
-
pcl::filters::Convolution<PointIn, PointOut>::convolveOneRowDense (int i, int j)
+Convolution<PointIn, PointOut>::convolveOneRowDense (int i, int j)
{
using namespace pcl::common;
PointOut result;
}
template <typename PointIn, typename PointOut> inline PointOut
-
pcl::filters::Convolution<PointIn, PointOut>::convolveOneColDense (int i, int j)
+Convolution<PointIn, PointOut>::convolveOneColDense (int i, int j)
{
using namespace pcl::common;
PointOut result;
}
template <typename PointIn, typename PointOut> inline PointOut
-
pcl::filters::Convolution<PointIn, PointOut>::convolveOneRowNonDense (int i, int j)
+Convolution<PointIn, PointOut>::convolveOneRowNonDense (int i, int j)
{
using namespace pcl::common;
PointOut result;
}
template <typename PointIn, typename PointOut> inline PointOut
-
pcl::filters::Convolution<PointIn, PointOut>::convolveOneColNonDense (int i, int j)
+Convolution<PointIn, PointOut>::convolveOneColNonDense (int i, int j)
{
using namespace pcl::common;
PointOut result;
return (result);
}
-namespace pcl
-{
- namespace filters
- {
- template<> pcl::PointXYZRGB
- Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense (int i, int j)
- {
- pcl::PointXYZRGB result;
- float r = 0, g = 0, b = 0;
- for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
- {
- result.x += (*input_) (l,j).x * kernel_[k];
- result.y += (*input_) (l,j).y * kernel_[k];
- result.z += (*input_) (l,j).z * kernel_[k];
- r += kernel_[k] * static_cast<float> ((*input_) (l,j).r);
- g += kernel_[k] * static_cast<float> ((*input_) (l,j).g);
- b += kernel_[k] * static_cast<float> ((*input_) (l,j).b);
- }
- result.r = static_cast<std::uint8_t> (r);
- result.g = static_cast<std::uint8_t> (g);
- result.b = static_cast<std::uint8_t> (b);
- return (result);
- }
+template<> pcl::PointXYZRGB
+Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense (int i, int j);
- template<> pcl::PointXYZRGB
- Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense (int i, int j)
- {
- pcl::PointXYZRGB result;
- float r = 0, g = 0, b = 0;
- for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
- {
- result.x += (*input_) (i,l).x * kernel_[k];
- result.y += (*input_) (i,l).y * kernel_[k];
- result.z += (*input_) (i,l).z * kernel_[k];
- r += kernel_[k] * static_cast<float> ((*input_) (i,l).r);
- g += kernel_[k] * static_cast<float> ((*input_) (i,l).g);
- b += kernel_[k] * static_cast<float> ((*input_) (i,l).b);
- }
- result.r = static_cast<std::uint8_t> (r);
- result.g = static_cast<std::uint8_t> (g);
- result.b = static_cast<std::uint8_t> (b);
- return (result);
- }
+template<> pcl::PointXYZRGB
+Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense (int i, int j);
- template<> pcl::PointXYZRGB
- Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense (int i, int j)
- {
- pcl::PointXYZRGB result;
- float weight = 0;
- float r = 0, g = 0, b = 0;
- for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
- {
- if (!isFinite ((*input_) (l,j)))
- continue;
- if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_)
- {
- result.x += (*input_) (l,j).x * kernel_[k]; result.y += (*input_) (l,j).y * kernel_[k]; result.z += (*input_) (l,j).z * kernel_[k];
- r+= kernel_[k] * static_cast<float> ((*input_) (l,j).r);
- g+= kernel_[k] * static_cast<float> ((*input_) (l,j).g);
- b+= kernel_[k] * static_cast<float> ((*input_) (l,j).b);
- weight += kernel_[k];
- }
- }
-
- if (weight == 0)
- result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
- else
- {
- weight = 1.f/weight;
- r*= weight; g*= weight; b*= weight;
- result.x*= weight; result.y*= weight; result.z*= weight;
- result.r = static_cast<std::uint8_t> (r);
- result.g = static_cast<std::uint8_t> (g);
- result.b = static_cast<std::uint8_t> (b);
- }
- return (result);
- }
+template<> pcl::PointXYZRGB
+Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense (int i, int j);
- template<> pcl::PointXYZRGB
- Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense (int i, int j)
- {
- pcl::PointXYZRGB result;
- float weight = 0;
- float r = 0, g = 0, b = 0;
- for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
- {
- if (!isFinite ((*input_) (i,l)))
- continue;
- if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_)
- {
- result.x += (*input_) (i,l).x * kernel_[k]; result.y += (*input_) (i,l).y * kernel_[k]; result.z += (*input_) (i,l).z * kernel_[k];
- r+= kernel_[k] * static_cast<float> ((*input_) (i,l).r);
- g+= kernel_[k] * static_cast<float> ((*input_) (i,l).g);
- b+= kernel_[k] * static_cast<float> ((*input_) (i,l).b);
- weight+= kernel_[k];
- }
- }
- if (weight == 0)
- result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
- else
- {
- weight = 1.f/weight;
- r*= weight; g*= weight; b*= weight;
- result.x*= weight; result.y*= weight; result.z*= weight;
- result.r = static_cast<std::uint8_t> (r);
- result.g = static_cast<std::uint8_t> (g);
- result.b = static_cast<std::uint8_t> (b);
- }
- return (result);
- }
+template<> pcl::PointXYZRGB
+Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense (int i, int j);
- ///////////////////////////////////////////////////////////////////////////////////////////////
- template<> pcl::RGB
- Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense (int i, int j)
- {
- pcl::RGB result;
- float r = 0, g = 0, b = 0;
- for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
- {
- r += kernel_[k] * static_cast<float> ((*input_) (l,j).r);
- g += kernel_[k] * static_cast<float> ((*input_) (l,j).g);
- b += kernel_[k] * static_cast<float> ((*input_) (l,j).b);
- }
- result.r = static_cast<std::uint8_t> (r);
- result.g = static_cast<std::uint8_t> (g);
- result.b = static_cast<std::uint8_t> (b);
- return (result);
- }
+template<> pcl::RGB
+Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense (int i, int j);
- template<> pcl::RGB
- Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense (int i, int j)
- {
- pcl::RGB result;
- float r = 0, g = 0, b = 0;
- for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
- {
- r += kernel_[k] * static_cast<float> ((*input_) (i,l).r);
- g += kernel_[k] * static_cast<float> ((*input_) (i,l).g);
- b += kernel_[k] * static_cast<float> ((*input_) (i,l).b);
- }
- result.r = static_cast<std::uint8_t> (r);
- result.g = static_cast<std::uint8_t> (g);
- result.b = static_cast<std::uint8_t> (b);
- return (result);
- }
+template<> pcl::RGB
+Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense (int i, int j);
- template<> pcl::RGB
-
Convolution<pcl::RGB, pcl::RGB>::convolveOneRowNonDense (int i, int j)
- {
- return (convolveOneRowDense (i,j));
- }
+template<> inline pcl::RGB
+Convolution<pcl::RGB, pcl::RGB>::convolveOneRowNonDense (int i, int j)
+{
+ return (convolveOneRowDense (i,j));
+}
- template<> pcl::RGB
-
Convolution<pcl::RGB, pcl::RGB>::convolveOneColNonDense (int i, int j)
- {
- return (convolveOneColDense (i,j));
- }
+template<> inline pcl::RGB
+Convolution<pcl::RGB, pcl::RGB>::convolveOneColNonDense (int i, int j)
+{
+ return (convolveOneColDense (i,j));
+}
- template<> void
- Convolution<pcl::RGB, pcl::RGB>::makeInfinite (pcl::RGB& p)
- {
- p.r = 0; p.g = 0; p.b = 0;
- }
- }
+template<> inline void
+Convolution<pcl::RGB, pcl::RGB>::makeInfinite (pcl::RGB& p)
+{
+ p.r = 0; p.g = 0; p.b = 0;
}
template <typename PointIn, typename PointOut> void
-
pcl::filters::Convolution<PointIn, PointOut>::convolve_rows (PointCloudOut& output)
+Convolution<PointIn, PointOut>::convolve_rows (PointCloudOut& output)
{
using namespace pcl::common;
int last = input_->width - half_width_;
if (input_->is_dense)
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(height, last, output, width) \
+ num_threads(threads_)
for(int j = 0; j < height; ++j)
{
for (int i = 0; i < half_width_; ++i)
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(height, last, output, width) \
+ num_threads(threads_)
for(int j = 0; j < height; ++j)
{
for (int i = 0; i < half_width_; ++i)
}
template <typename PointIn, typename PointOut> void
-
pcl::filters::Convolution<PointIn, PointOut>::convolve_rows_duplicate (PointCloudOut& output)
+Convolution<PointIn, PointOut>::convolve_rows_duplicate (PointCloudOut& output)
{
using namespace pcl::common;
int w = last - 1;
if (input_->is_dense)
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(height, last, output, w, width) \
+ num_threads(threads_)
for(int j = 0; j < height; ++j)
{
for (int i = half_width_; i < last; ++i)
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(height, last, output, w, width) \
+ num_threads(threads_)
for(int j = 0; j < height; ++j)
{
for (int i = half_width_; i < last; ++i)
}
template <typename PointIn, typename PointOut> void
-
pcl::filters::Convolution<PointIn, PointOut>::convolve_rows_mirror (PointCloudOut& output)
+Convolution<PointIn, PointOut>::convolve_rows_mirror (PointCloudOut& output)
{
using namespace pcl::common;
int w = last - 1;
if (input_->is_dense)
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(height, last, output, w, width) \
+ num_threads(threads_)
for(int j = 0; j < height; ++j)
{
for (int i = half_width_; i < last; ++i)
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(height, last, output, w, width) \
+ num_threads(threads_)
for(int j = 0; j < height; ++j)
{
for (int i = half_width_; i < last; ++i)
}
template <typename PointIn, typename PointOut> void
-
pcl::filters::Convolution<PointIn, PointOut>::convolve_cols (PointCloudOut& output)
+Convolution<PointIn, PointOut>::convolve_cols (PointCloudOut& output)
{
using namespace pcl::common;
int last = input_->height - half_width_;
if (input_->is_dense)
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(height, last, output, width) \
+ num_threads(threads_)
for(int i = 0; i < width; ++i)
{
for (int j = 0; j < half_width_; ++j)
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(height, last, output, width) \
+ num_threads(threads_)
for(int i = 0; i < width; ++i)
{
for (int j = 0; j < half_width_; ++j)
}
template <typename PointIn, typename PointOut> void
-
pcl::filters::Convolution<PointIn, PointOut>::convolve_cols_duplicate (PointCloudOut& output)
+Convolution<PointIn, PointOut>::convolve_cols_duplicate (PointCloudOut& output)
{
using namespace pcl::common;
int h = last -1;
if (input_->is_dense)
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(h, height, last, output, width) \
+ num_threads(threads_)
for(int i = 0; i < width; ++i)
{
for (int j = half_width_; j < last; ++j)
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(h, height, last, output, width) \
+ num_threads(threads_)
for(int i = 0; i < width; ++i)
{
for (int j = half_width_; j < last; ++j)
}
template <typename PointIn, typename PointOut> void
-
pcl::filters::Convolution<PointIn, PointOut>::convolve_cols_mirror (PointCloudOut& output)
+Convolution<PointIn, PointOut>::convolve_cols_mirror (PointCloudOut& output)
{
using namespace pcl::common;
int h = last -1;
if (input_->is_dense)
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(h, height, last, output, width) \
+ num_threads(threads_)
for(int i = 0; i < width; ++i)
{
for (int j = half_width_; j < last; ++j)
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(h, height, last, output, width) \
+ num_threads(threads_)
for(int i = 0; i < width; ++i)
{
for (int j = half_width_; j < last; ++j)
}
}
-#endif //PCL_FILTERS_CONVOLUTION_IMPL_HPP
+} // namespace filters
+} // namespace pcl
+
#include <pcl/pcl_config.h>
#include <pcl/point_types.h>
-#include <pcl/common/point_operators.h>
#include <cmath>
#include <cstdint>
std::vector<int> nn_indices;
std::vector<float> nn_distances;
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (nn_indices, nn_distances) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(nn_indices, nn_distances) \
+ num_threads(threads_)
for (std::int64_t point_idx = 0; point_idx < static_cast<std::int64_t> (surface_->size ()); ++point_idx)
{
const PointInT& point_in = surface_->points [point_idx];
#include <pcl/filters/crop_box.h>
#include <pcl/common/io.h>
-///////////////////////////////////////////////////////////////////////////////
-template<typename PointT> void
-pcl::CropBox<PointT>::applyFilter (PointCloud &output)
-{
- std::vector<int> indices;
- if (keep_organized_)
- {
- bool temp = extract_removed_indices_;
- extract_removed_indices_ = true;
- applyFilter (indices);
- extract_removed_indices_ = temp;
-
- output = *input_;
- for (const auto ri : *removed_indices_) // ri = removed index
- output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_;
- if (!std::isfinite (user_filter_value_))
- output.is_dense = false;
- }
- else
- {
- output.is_dense = true;
- applyFilter (indices);
- pcl::copyPointCloud (*input_, indices, output);
- }
-}
-
///////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
* $Id$
*
*/
+
#ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_
#define PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_
#include <pcl/common/io.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointT> void
-
pcl::FastBilateralFilter<PointT>::applyFilter (PointCloud &output)
+FastBilateralFilter<PointT>::applyFilter (PointCloud &output)
{
if (!input_->isOrganized ())
{
}
-
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> std::size_t
-
pcl::FastBilateralFilter<PointT>::Array3D::clamp (const std::size_t min_value,
- const std::size_t max_value,
- const std::size_t x)
+FastBilateralFilter<PointT>::Array3D::clamp (const std::size_t min_value,
+ const std::size_t max_value,
+ const std::size_t x)
{
if (x >= min_value && x <= max_value)
{
return (max_value);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> Eigen::Vector2f
-
pcl::FastBilateralFilter<PointT>::Array3D::trilinear_interpolation (const float x,
- const float y,
- const float z)
+FastBilateralFilter<PointT>::Array3D::trilinear_interpolation (const float x,
+ const float y,
+ const float z)
{
const std::size_t x_index = clamp (0, x_dim_ - 1, static_cast<std::size_t> (x));
const std::size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1);
x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index);
}
+} // namespace pcl
+
#endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ */
+
PCL_WARN ("[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n");
return;
}
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(base_min, base_max, output) \
+ num_threads(threads_)
for (long int i = 0; i < static_cast<long int> (output.size ()); ++i)
if (!std::isfinite (output.at(i).z))
output.at(i).z = base_max;
const std::size_t small_depth = static_cast<std::size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z;
Array3D data (small_width, small_height, small_depth);
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(base_min, data, output) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(base_min, data, output, small_height, small_width) \
+ num_threads(threads_)
#endif
for (long int i = 0; i < static_cast<long int> (small_width * small_height); ++i)
{
{
Array3D* current_buffer = (n_iter % 2 == 1 ? &buffer : &data);
Array3D* current_data =(n_iter % 2 == 1 ? &data : &buffer);
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(current_buffer, current_data, dim, offset) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(current_buffer, current_data, dim, offset, small_depth, small_height, small_width) \
+ num_threads(threads_)
#endif
for(long int i = 0; i < static_cast<long int> ((small_width - 2)*(small_height - 2)); ++i)
{
for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
*d /= ((*d)[0] != 0) ? (*d)[1] : 1;
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(base_min, data, output) \
+ num_threads(threads_)
for (long int i = 0; i < static_cast<long int> (input_->size ()); ++i)
{
std::size_t x = static_cast<std::size_t> (i % input_->width);
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(base_min, data, output) \
+ num_threads(threads_)
for (long i = 0; i < static_cast<long int> (input_->size ()); ++i)
{
std::size_t x = static_cast<std::size_t> (i % input_->width);
*
*/
-#ifndef PCL_FILTERS_IMPL_FILTER_H_
-#define PCL_FILTERS_IMPL_FILTER_H_
+#pragma once
#include <pcl/pcl_macros.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/filters/filter.h>
//////////////////////////////////////////////////////////////////////////
#define PCL_INSTANTIATE_removeNaNFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud<T>(const pcl::PointCloud<T>&, pcl::PointCloud<T>&, std::vector<int>&);
#define PCL_INSTANTIATE_removeNaNNormalsFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNNormalsFromPointCloud<T>(const pcl::PointCloud<T>&, pcl::PointCloud<T>&, std::vector<int>&);
-#endif // PCL_FILTERS_IMPL_FILTER_H_
-
}
}
+template<typename PointT> void
+pcl::FilterIndices<PointT>::applyFilter (PointCloud &output)
+{
+ std::vector<int> indices;
+ if (keep_organized_)
+ {
+ if (!extract_removed_indices_)
+ {
+ PCL_WARN ("[pcl::FilterIndices<PointT>::applyFilter] extract_removed_indices_ was set to 'true' to keep the point cloud organized.\n");
+ extract_removed_indices_ = true;
+ }
+ applyFilter (indices);
+
+ output = *input_;
+
+ // To preserve legacy behavior, only coordinates xyz are filtered.
+ // Copying a PointXYZ initialized with the user_filter_value_ into a generic
+ // PointT, ensures only the xyz coordinates, if they exist at destination,
+ // are overwritten.
+ const PointXYZ ufv (user_filter_value_, user_filter_value_, user_filter_value_);
+ for (const auto ri : *removed_indices_) // ri = removed index
+ copyPoint(ufv, output[ri]);
+ if (!std::isfinite (user_filter_value_))
+ output.is_dense = false;
+ }
+ else
+ {
+ output.is_dense = true;
+ applyFilter (indices);
+ pcl::copyPointCloud (*input_, indices, output);
+ }
+}
+
+
#define PCL_INSTANTIATE_removeNanFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud<T>(const pcl::PointCloud<T>&, std::vector<int>&);
+#define PCL_INSTANTIATE_FilterIndices(T) template class PCL_EXPORTS pcl::FilterIndices<T>;
#endif // PCL_FILTERS_IMPL_FILTER_INDICES_H_
#include <pcl/common/io.h>
#include <vector>
-///////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::FrustumCulling<PointT>::applyFilter (PointCloud& output)
-{
- std::vector<int> indices;
- if (keep_organized_)
- {
- bool temp = extract_removed_indices_;
- extract_removed_indices_ = true;
- applyFilter (indices);
- extract_removed_indices_ = temp;
- copyPointCloud (*input_, output);
-
- for (std::size_t rii = 0; rii < removed_indices_->size (); ++rii)
- {
- PointT &pt_to_remove = output.at ((*removed_indices_)[rii]);
- pt_to_remove.x = pt_to_remove.y = pt_to_remove.z = user_filter_value_;
- if (!std::isfinite (user_filter_value_))
- output.is_dense = false;
- }
- }
- else
- {
- output.is_dense = true;
- applyFilter (indices);
- copyPointCloud (*input_, indices, output);
- }
-}
-
///////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::FrustumCulling<PointT>::applyFilter (std::vector<int> &indices)
*
*/
-#ifndef PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_
-#define PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_
+#pragma once
#include <pcl/common/io.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/filters/local_maximum.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/ModelCoefficients.h>
#define PCL_INSTANTIATE_LocalMaximum(T) template class PCL_EXPORTS pcl::LocalMaximum<T>;
-#endif // PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_
-
*
*/
-#ifndef PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_
-#define PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_
+#pragma once
#include <pcl/filters/median_filter.h>
#include <pcl/common/io.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
template <typename PointT> void
pcl::MedianFilter<PointT>::applyFilter (PointCloud &output)
}
}
-
-#endif /* PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ */
* POSSIBILITY OF SUCH DAMAGE.
*/
-#ifndef PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_
-#define PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_
+#pragma once
#include <pcl/filters/model_outlier_removal.h>
#include <pcl/common/io.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/sample_consensus/sac_model_circle.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/sample_consensus/sac_model_cone.h>
return (true);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::ModelOutlierRemoval<PointT>::applyFilter (PointCloud &output)
-{
- std::vector<int> indices;
- if (keep_organized_)
- {
- bool temp = extract_removed_indices_;
- extract_removed_indices_ = true;
- applyFilterIndices (indices);
- extract_removed_indices_ = temp;
-
- output = *input_;
- for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
- output.points[ (*removed_indices_)[rii]].x = output.points[ (*removed_indices_)[rii]].y = output.points[ (*removed_indices_)[rii]].z = user_filter_value_;
- if (!std::isfinite (user_filter_value_))
- output.is_dense = false;
- }
- else
- {
- applyFilterIndices (indices);
- copyPointCloud (*input_, indices, output);
- }
-}
-
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::ModelOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
#define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval<T>;
-#endif // PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_
#include <pcl/filters/morphological_filter.h>
#include <pcl/octree/octree_search.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+namespace pcl
+{
template <typename PointT> void
-
pcl::applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
- float resolution, const int morphological_operator,
-
pcl::PointCloud<PointT> &cloud_out)
+applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
+ float resolution, const int morphological_operator,
+ pcl::PointCloud<PointT> &cloud_out)
{
if (cloud_in->empty ())
return;
return;
}
+} // namespace pcl
+
#define PCL_INSTANTIATE_applyMorphologicalOperator(T) template PCL_EXPORTS void pcl::applyMorphologicalOperator<T> (const pcl::PointCloud<T>::ConstPtr &, float, const int, pcl::PointCloud<T> &);
#endif //#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_
-
return false;
}
- boost::mt19937 rng (static_cast<unsigned int> (seed_));
- boost::uniform_int<unsigned int> uniform_distrib (0, unsigned (input_->size ()));
- delete rng_uniform_distribution_;
- rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_int<unsigned int> > (rng, uniform_distrib);
-
+ rng_.seed (seed_);
return (true);
}
-///////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename NormalT> void
-pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (PointCloud &output)
-{
- std::vector<int> indices;
- if (keep_organized_)
- {
- bool temp = extract_removed_indices_;
- extract_removed_indices_ = true;
- applyFilter (indices);
- extract_removed_indices_ = temp;
-
- output = *input_;
- for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
- output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
- if (!std::isfinite (user_filter_value_))
- output.is_dense = false;
- }
- else
- {
- output.is_dense = true;
- applyFilter (indices);
- pcl::copyPointCloud (*input_, indices, output);
- }
-}
-
///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename NormalT> bool
pcl::NormalSpaceSampling<PointT, NormalT>::isEntireBinSampled (boost::dynamic_bitset<> &array,
///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename NormalT> unsigned int
-pcl::NormalSpaceSampling<PointT, NormalT>::findBin (const float *normal, unsigned int)
+pcl::NormalSpaceSampling<PointT, NormalT>::findBin (const float *normal)
{
- unsigned int bin_number = 0;
- // Holds the bin numbers for direction cosines in x,y,z directions
- unsigned int t[3] = {0,0,0};
-
- // dcos is the direction cosine.
- float dcos = 0.0;
- float bin_size = 0.0;
- // max_cos and min_cos are the maximum and minimum values of std::cos(theta) respectively
- float max_cos = 1.0;
- float min_cos = -1.0;
-
-// dcos = std::cos (normal[0]);
- dcos = normal[0];
- bin_size = (max_cos - min_cos) / static_cast<float> (binsx_);
-
- // Finding bin number for direction cosine in x direction
- unsigned int k = 0;
- for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
- {
- if (dcos >= i && dcos <= (i+bin_size))
- {
- break;
- }
- }
- t[0] = k;
-
-// dcos = std::cos (normal[1]);
- dcos = normal[1];
- bin_size = (max_cos - min_cos) / static_cast<float> (binsy_);
-
- // Finding bin number for direction cosine in y direction
- k = 0;
- for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
- {
- if (dcos >= i && dcos <= (i+bin_size))
- {
- break;
- }
- }
- t[1] = k;
-
-// dcos = std::cos (normal[2]);
- dcos = normal[2];
- bin_size = (max_cos - min_cos) / static_cast<float> (binsz_);
-
- // Finding bin number for direction cosine in z direction
- k = 0;
- for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
- {
- if (dcos >= i && dcos <= (i+bin_size))
- {
- break;
- }
- }
- t[2] = k;
-
- bin_number = t[0] * (binsy_*binsz_) + t[1] * binsz_ + t[2];
- return bin_number;
+ const unsigned ix = static_cast<unsigned> (std::round (0.5f * (binsx_ - 1.f) * (normal[0] + 1.f)));
+ const unsigned iy = static_cast<unsigned> (std::round (0.5f * (binsy_ - 1.f) * (normal[1] + 1.f)));
+ const unsigned iz = static_cast<unsigned> (std::round (0.5f * (binsz_ - 1.f) * (normal[2] + 1.f)));
+ return ix * (binsy_*binsz_) + iy * binsz_ + iz;
}
///////////////////////////////////////////////////////////////////////////////
for (unsigned int i = 0; i < n_bins; i++)
normals_hg.emplace_back();
- for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
+ for (const auto index : *indices_)
{
- unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins);
- normals_hg[bin_number].push_back (*it);
+ unsigned int bin_number = findBin ((*input_normals_)[index].normal);
+ normals_hg[bin_number].push_back (index);
}
unsigned int pos = 0;
unsigned int random_index = 0;
+ std::uniform_int_distribution<unsigned> rng_uniform_distribution (0u, M - 1u);
// Picking up a sample at random from jth bin
do
{
- random_index = static_cast<unsigned int> ((*rng_uniform_distribution_) () % M);
+ random_index = rng_uniform_distribution (rng_);
pos = start_index[j] + random_index;
} while (is_sampled_flag.test (pos));
#include <pcl/filters/passthrough.h>
#include <pcl/common/io.h>
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::PassThrough<PointT>::applyFilter (PointCloud &output)
-{
- std::vector<int> indices;
- if (keep_organized_)
- {
- bool temp = extract_removed_indices_;
- extract_removed_indices_ = true;
- applyFilterIndices (indices);
- extract_removed_indices_ = temp;
-
- output = *input_;
- for (const auto ri : *removed_indices_) // ri = removed index
- output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_;
- if (!std::isfinite (user_filter_value_))
- output.is_dense = false;
- }
- else
- {
- output.is_dense = true;
- applyFilterIndices (indices);
- copyPointCloud (*input_, indices, output);
- }
-}
-
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::PassThrough<PointT>::applyFilterIndices (std::vector<int> &indices)
if (indices.empty ())
{
clipped.reserve (cloud_in.size ());
- /*
-#if 0
- Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float));
- Eigen::VectorXf distances = plane_params_.transpose () * points;
- for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
- {
- if (distances (rIdx, 0) >= -plane_params_[3])
- clipped.push_back (rIdx);
- }
-#else
- Eigen::Matrix4Xf points (4, cloud_in.size ());
- for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
- {
- points (0, rIdx) = cloud_in[rIdx].x;
- points (1, rIdx) = cloud_in[rIdx].y;
- points (2, rIdx) = cloud_in[rIdx].z;
- points (3, rIdx) = 1;
- }
- Eigen::VectorXf distances = plane_params_.transpose () * points;
- for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
- {
- if (distances (rIdx, 0) >= 0)
- clipped.push_back (rIdx);
- }
-
-#endif
- //std::cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << std::endl;
+// #if 0
+// Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float));
+// Eigen::VectorXf distances = plane_params_.transpose () * points;
+// for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
+// {
+// if (distances (rIdx, 0) >= -plane_params_[3])
+// clipped.push_back (rIdx);
+// }
+// #else
+// Eigen::Matrix4Xf points (4, cloud_in.size ());
+// for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
+// {
+// points (0, rIdx) = cloud_in[rIdx].x;
+// points (1, rIdx) = cloud_in[rIdx].y;
+// points (2, rIdx) = cloud_in[rIdx].z;
+// points (3, rIdx) = 1;
+// }
+// Eigen::VectorXf distances = plane_params_.transpose () * points;
+// for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
+// {
+// if (distances (rIdx, 0) >= 0)
+// clipped.push_back (rIdx);
+// }
+//
+// #endif
+//
+// //std::cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << std::endl;
+//
+// //std::cout << "distances: " << distances.rows () << " x " << distances.cols () << std::endl;
- //std::cout << "distances: " << distances.rows () << " x " << distances.cols () << std::endl;
- /*/
for (unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
if (clipPoint3D (cloud_in[pIdx]))
clipped.push_back (pIdx);
- //*/
}
else
{
*
*/
+
#ifndef PCL_FILTERS_IMPL_PYRAMID_HPP
#define PCL_FILTERS_IMPL_PYRAMID_HPP
+
+namespace pcl
+{
+
+namespace filters
+{
+
template <typename PointT> bool
-
pcl::filters::Pyramid<PointT>::initCompute ()
+Pyramid<PointT>::initCompute ()
{
if (!input_->isOrganized ())
{
PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
return (false);
}
-
+
if (levels_ < 2)
{
PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
// std::size_t ratio (std::pow (2, levels_));
// std::size_t last_width = input_->width / ratio;
// std::size_t last_height = input_->height / ratio;
-
+
if (levels_ > 4)
{
PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!", getClassName ().c_str ());
return (false);
}
-
+
if (large_)
{
Eigen::VectorXf k (5);
if (threshold_ != std::numeric_limits<float>::infinity ())
threshold_ *= threshold_;
}
-
+
return (true);
}
template <typename PointT> void
-
pcl::filters::Pyramid<PointT>::compute (std::vector<PointCloudPtr>& output)
+Pyramid<PointT>::compute (std::vector<PointCloudPtr>& output)
{
std::cout << "compute" << std::endl;
if (!initCompute ())
output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
const PointCloud<PointT> &previous = *output[l-1];
PointCloud<PointT> &next = *output[l];
-#ifdef _OPENMP
-#pragma omp parallel for shared (next) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(next) \
+ num_threads(threads_)
for(int i=0; i < next.height; ++i)
{
for(int j=0; j < next.width; ++j)
{
for(int m=0; m < kernel_rows; ++m)
{
- int mm = kernel_rows - 1 - m;
- for(int n=0; n < kernel_cols; ++n)
+ int mm = kernel_rows - 1 - m;
+ for(int n=0; n < kernel_cols; ++n)
{
int nn = kernel_cols - 1 - n;
int ii = 2*i + (m - kernel_center_y);
int jj = 2*j + (n - kernel_center_x);
-
+
if (ii < 0) ii = 0;
if (ii >= previous.height) ii = previous.height - 1;
if (jj < 0) jj = 0;
output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
const PointCloud<PointT> &previous = *output[l-1];
PointCloud<PointT> &next = *output[l];
-#ifdef _OPENMP
-#pragma omp parallel for shared (next) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(next) \
+ num_threads(threads_)
for(int i=0; i < next.height; ++i)
{
for(int j=0; j < next.width; ++j)
}
}
}
- }
+ }
}
-namespace pcl
+
+template <> void
+Pyramid<pcl::PointXYZRGB>::compute (std::vector<Pyramid<pcl::PointXYZRGB>::PointCloudPtr> &output)
{
- namespace filters
+ std::cout << "PointXYZRGB" << std::endl;
+ if (!initCompute ())
{
- template <> void
- Pyramid<pcl::PointXYZRGB>::compute (std::vector<Pyramid<pcl::PointXYZRGB>::PointCloudPtr> &output)
- {
- std::cout << "PointXYZRGB" << std::endl;
- if (!initCompute ())
- {
- PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
- return;
- }
-
- int kernel_rows = static_cast<int> (kernel_.rows ());
- int kernel_cols = static_cast<int> (kernel_.cols ());
- int kernel_center_x = kernel_cols / 2;
- int kernel_center_y = kernel_rows / 2;
+ PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
+ return;
+ }
- output.resize (levels_ + 1);
- output[0].reset (new pcl::PointCloud<pcl::PointXYZRGB>);
- *(output[0]) = *input_;
+ int kernel_rows = static_cast<int> (kernel_.rows ());
+ int kernel_cols = static_cast<int> (kernel_.cols ());
+ int kernel_center_x = kernel_cols / 2;
+ int kernel_center_y = kernel_rows / 2;
+
+ output.resize (levels_ + 1);
+ output[0].reset (new pcl::PointCloud<pcl::PointXYZRGB>);
+ *(output[0]) = *input_;
- if (input_->is_dense)
+ if (input_->is_dense)
+ {
+ for (int l = 1; l <= levels_; ++l)
+ {
+ output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
+ const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
+ PointCloud<pcl::PointXYZRGB> &next = *output[l];
+#pragma omp parallel for \
+ default(none) \
+ shared(next) \
+ num_threads(threads_)
+ for(int i=0; i < next.height; ++i) // rows
{
- for (int l = 1; l <= levels_; ++l)
+ for(int j=0; j < next.width; ++j) // columns
{
- output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
- const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
- PointCloud<pcl::PointXYZRGB> &next = *output[l];
-#ifdef _OPENMP
-#pragma omp parallel for shared (next) num_threads(threads_)
-#endif
- for(int i=0; i < next.height; ++i) // rows
+ float r = 0, g = 0, b = 0;
+ for(int m=0; m < kernel_rows; ++m) // kernel rows
{
- for(int j=0; j < next.width; ++j) // columns
+ int mm = kernel_rows - 1 - m; // row index of flipped kernel
+ for(int n=0; n < kernel_cols; ++n) // kernel columns
{
- float r = 0, g = 0, b = 0;
- for(int m=0; m < kernel_rows; ++m) // kernel rows
- {
- int mm = kernel_rows - 1 - m; // row index of flipped kernel
- for(int n=0; n < kernel_cols; ++n) // kernel columns
- {
- int nn = kernel_cols - 1 - n; // column index of flipped kernel
- // index of input signal, used for checking boundary
- int ii = 2*i + (m - kernel_center_y);
- int jj = 2*j + (n - kernel_center_x);
-
- // ignore input samples which are out of bound
- if (ii < 0) ii = 0;
- if (ii >= previous.height) ii = previous.height - 1;
- if (jj < 0) jj = 0;
- if (jj >= previous.width) jj = previous.width - 1;
- next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
- next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
- next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
- b += previous.at (jj,ii).b * kernel_ (mm,nn);
- g += previous.at (jj,ii).g * kernel_ (mm,nn);
- r += previous.at (jj,ii).r * kernel_ (mm,nn);
- }
- }
- next.at (j,i).b = static_cast<std::uint8_t> (b);
- next.at (j,i).g = static_cast<std::uint8_t> (g);
- next.at (j,i).r = static_cast<std::uint8_t> (r);
+ int nn = kernel_cols - 1 - n; // column index of flipped kernel
+ // index of input signal, used for checking boundary
+ int ii = 2*i + (m - kernel_center_y);
+ int jj = 2*j + (n - kernel_center_x);
+
+ // ignore input samples which are out of bound
+ if (ii < 0) ii = 0;
+ if (ii >= previous.height) ii = previous.height - 1;
+ if (jj < 0) jj = 0;
+ if (jj >= previous.width) jj = previous.width - 1;
+ next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
+ next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
+ next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
+ b += previous.at (jj,ii).b * kernel_ (mm,nn);
+ g += previous.at (jj,ii).g * kernel_ (mm,nn);
+ r += previous.at (jj,ii).r * kernel_ (mm,nn);
}
}
+ next.at (j,i).b = static_cast<std::uint8_t> (b);
+ next.at (j,i).g = static_cast<std::uint8_t> (g);
+ next.at (j,i).r = static_cast<std::uint8_t> (r);
}
}
- else
+ }
+ }
+ else
+ {
+ for (int l = 1; l <= levels_; ++l)
+ {
+ output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
+ const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
+ PointCloud<pcl::PointXYZRGB> &next = *output[l];
+#pragma omp parallel for \
+ default(none) \
+ shared(next) \
+ num_threads(threads_)
+ for(int i=0; i < next.height; ++i)
{
- for (int l = 1; l <= levels_; ++l)
+ for(int j=0; j < next.width; ++j)
{
- output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
- const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
- PointCloud<pcl::PointXYZRGB> &next = *output[l];
-#ifdef _OPENMP
-#pragma omp parallel for shared (next) num_threads(threads_)
-#endif
- for(int i=0; i < next.height; ++i)
+ float weight = 0;
+ float r = 0, g = 0, b = 0;
+ for(int m=0; m < kernel_rows; ++m)
{
- for(int j=0; j < next.width; ++j)
+ int mm = kernel_rows - 1 - m;
+ for(int n=0; n < kernel_cols; ++n)
{
- float weight = 0;
- float r = 0, g = 0, b = 0;
- for(int m=0; m < kernel_rows; ++m)
- {
- int mm = kernel_rows - 1 - m;
- for(int n=0; n < kernel_cols; ++n)
- {
- int nn = kernel_cols - 1 - n;
- int ii = 2*i + (m - kernel_center_y);
- int jj = 2*j + (n - kernel_center_x);
- if (ii < 0) ii = 0;
- if (ii >= previous.height) ii = previous.height - 1;
- if (jj < 0) jj = 0;
- if (jj >= previous.width) jj = previous.width - 1;
- if (!isFinite (previous.at (jj,ii)))
- continue;
- if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
- {
- next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
- next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
- next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
- b += previous.at (jj,ii).b * kernel_ (mm,nn);
- g += previous.at (jj,ii).g * kernel_ (mm,nn);
- r += previous.at (jj,ii).r * kernel_ (mm,nn);
- weight+= kernel_ (mm,nn);
- }
- }
- }
- if (weight == 0)
- nullify (next.at (j,i));
- else
+ int nn = kernel_cols - 1 - n;
+ int ii = 2*i + (m - kernel_center_y);
+ int jj = 2*j + (n - kernel_center_x);
+ if (ii < 0) ii = 0;
+ if (ii >= previous.height) ii = previous.height - 1;
+ if (jj < 0) jj = 0;
+ if (jj >= previous.width) jj = previous.width - 1;
+ if (!isFinite (previous.at (jj,ii)))
+ continue;
+ if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
{
- weight = 1.f/weight;
- r*= weight; g*= weight; b*= weight;
- next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight;
- next.at (j,i).b = static_cast<std::uint8_t> (b);
- next.at (j,i).g = static_cast<std::uint8_t> (g);
- next.at (j,i).r = static_cast<std::uint8_t> (r);
+ next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
+ next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
+ next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
+ b += previous.at (jj,ii).b * kernel_ (mm,nn);
+ g += previous.at (jj,ii).g * kernel_ (mm,nn);
+ r += previous.at (jj,ii).r * kernel_ (mm,nn);
+ weight+= kernel_ (mm,nn);
}
}
}
+ if (weight == 0)
+ nullify (next.at (j,i));
+ else
+ {
+ weight = 1.f/weight;
+ r*= weight; g*= weight; b*= weight;
+ next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight;
+ next.at (j,i).b = static_cast<std::uint8_t> (b);
+ next.at (j,i).g = static_cast<std::uint8_t> (g);
+ next.at (j,i).r = static_cast<std::uint8_t> (r);
+ }
}
- }
- }
-
- template <> void
- Pyramid<pcl::PointXYZRGBA>::compute (std::vector<Pyramid<pcl::PointXYZRGBA>::PointCloudPtr> &output)
- {
- std::cout << "PointXYZRGBA" << std::endl;
- if (!initCompute ())
- {
- PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
- return;
}
-
- int kernel_rows = static_cast<int> (kernel_.rows ());
- int kernel_cols = static_cast<int> (kernel_.cols ());
- int kernel_center_x = kernel_cols / 2;
- int kernel_center_y = kernel_rows / 2;
+ }
+ }
+}
+
+template <> void
+Pyramid<pcl::PointXYZRGBA>::compute (std::vector<Pyramid<pcl::PointXYZRGBA>::PointCloudPtr> &output)
+{
+ std::cout << "PointXYZRGBA" << std::endl;
+ if (!initCompute ())
+ {
+ PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
+ return;
+ }
+
+ int kernel_rows = static_cast<int> (kernel_.rows ());
+ int kernel_cols = static_cast<int> (kernel_.cols ());
+ int kernel_center_x = kernel_cols / 2;
+ int kernel_center_y = kernel_rows / 2;
- output.resize (levels_ + 1);
-
output[0].reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
- *(output[0]) = *input_;
+ output.resize (levels_ + 1);
+ output[0].reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
+ *(output[0]) = *input_;
- if (input_->is_dense)
+ if (input_->is_dense)
+ {
+ for (int l = 1; l <= levels_; ++l)
+ {
+ output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
+ const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
+ PointCloud<pcl::PointXYZRGBA> &next = *output[l];
+#pragma omp parallel for \
+ default(none) \
+ shared(next) \
+ num_threads(threads_)
+ for(int i=0; i < next.height; ++i) // rows
{
- for (int l = 1; l <= levels_; ++l)
+ for(int j=0; j < next.width; ++j) // columns
{
- output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
- const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
- PointCloud<pcl::PointXYZRGBA> &next = *output[l];
-#ifdef _OPENMP
-#pragma omp parallel for shared (next) num_threads(threads_)
-#endif
- for(int i=0; i < next.height; ++i) // rows
+ float r = 0, g = 0, b = 0, a = 0;
+ for(int m=0; m < kernel_rows; ++m) // kernel rows
{
- for(int j=0; j < next.width; ++j) // columns
+ int mm = kernel_rows - 1 - m; // row index of flipped kernel
+ for(int n=0; n < kernel_cols; ++n) // kernel columns
{
- float r = 0, g = 0, b = 0, a = 0;
- for(int m=0; m < kernel_rows; ++m) // kernel rows
- {
- int mm = kernel_rows - 1 - m; // row index of flipped kernel
- for(int n=0; n < kernel_cols; ++n) // kernel columns
- {
- int nn = kernel_cols - 1 - n; // column index of flipped kernel
- // index of input signal, used for checking boundary
- int ii = 2*i + (m - kernel_center_y);
- int jj = 2*j + (n - kernel_center_x);
-
- // ignore input samples which are out of bound
- if (ii < 0) ii = 0;
- if (ii >= previous.height) ii = previous.height - 1;
- if (jj < 0) jj = 0;
- if (jj >= previous.width) jj = previous.width - 1;
- next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
- next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
- next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
- b += previous.at (jj,ii).b * kernel_ (mm,nn);
- g += previous.at (jj,ii).g * kernel_ (mm,nn);
- r += previous.at (jj,ii).r * kernel_ (mm,nn);
- a += previous.at (jj,ii).a * kernel_ (mm,nn);
- }
- }
- next.at (j,i).b = static_cast<std::uint8_t> (b);
- next.at (j,i).g = static_cast<std::uint8_t> (g);
- next.at (j,i).r = static_cast<std::uint8_t> (r);
- next.at (j,i).a = static_cast<std::uint8_t> (a);
+ int nn = kernel_cols - 1 - n; // column index of flipped kernel
+ // index of input signal, used for checking boundary
+ int ii = 2*i + (m - kernel_center_y);
+ int jj = 2*j + (n - kernel_center_x);
+
+ // ignore input samples which are out of bound
+ if (ii < 0) ii = 0;
+ if (ii >= previous.height) ii = previous.height - 1;
+ if (jj < 0) jj = 0;
+ if (jj >= previous.width) jj = previous.width - 1;
+ next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
+ next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
+ next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
+ b += previous.at (jj,ii).b * kernel_ (mm,nn);
+ g += previous.at (jj,ii).g * kernel_ (mm,nn);
+ r += previous.at (jj,ii).r * kernel_ (mm,nn);
+ a += previous.at (jj,ii).a * kernel_ (mm,nn);
}
}
+ next.at (j,i).b = static_cast<std::uint8_t> (b);
+ next.at (j,i).g = static_cast<std::uint8_t> (g);
+ next.at (j,i).r = static_cast<std::uint8_t> (r);
+ next.at (j,i).a = static_cast<std::uint8_t> (a);
}
}
- else
+ }
+ }
+ else
+ {
+ for (int l = 1; l <= levels_; ++l)
+ {
+ output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
+ const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
+ PointCloud<pcl::PointXYZRGBA> &next = *output[l];
+#pragma omp parallel for \
+ default(none) \
+ shared(next) \
+ num_threads(threads_)
+ for(int i=0; i < next.height; ++i)
{
- for (int l = 1; l <= levels_; ++l)
+ for(int j=0; j < next.width; ++j)
{
- output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
- const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
- PointCloud<pcl::PointXYZRGBA> &next = *output[l];
-#ifdef _OPENMP
-#pragma omp parallel for shared (next) num_threads(threads_)
-#endif
- for(int i=0; i < next.height; ++i)
+ float weight = 0;
+ float r = 0, g = 0, b = 0, a = 0;
+ for(int m=0; m < kernel_rows; ++m)
{
- for(int j=0; j < next.width; ++j)
+ int mm = kernel_rows - 1 - m;
+ for(int n=0; n < kernel_cols; ++n)
{
- float weight = 0;
- float r = 0, g = 0, b = 0, a = 0;
- for(int m=0; m < kernel_rows; ++m)
- {
- int mm = kernel_rows - 1 - m;
- for(int n=0; n < kernel_cols; ++n)
- {
- int nn = kernel_cols - 1 - n;
- int ii = 2*i + (m - kernel_center_y);
- int jj = 2*j + (n - kernel_center_x);
- if (ii < 0) ii = 0;
- if (ii >= previous.height) ii = previous.height - 1;
- if (jj < 0) jj = 0;
- if (jj >= previous.width) jj = previous.width - 1;
- if (!isFinite (previous.at (jj,ii)))
- continue;
- if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
- {
- next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
- next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
- next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
- b += previous.at (jj,ii).b * kernel_ (mm,nn);
- g += previous.at (jj,ii).g * kernel_ (mm,nn);
- r += previous.at (jj,ii).r * kernel_ (mm,nn);
- a += previous.at (jj,ii).a * kernel_ (mm,nn);
- weight+= kernel_ (mm,nn);
- }
- }
- }
- if (weight == 0)
- nullify (next.at (j,i));
- else
+ int nn = kernel_cols - 1 - n;
+ int ii = 2*i + (m - kernel_center_y);
+ int jj = 2*j + (n - kernel_center_x);
+ if (ii < 0) ii = 0;
+ if (ii >= previous.height) ii = previous.height - 1;
+ if (jj < 0) jj = 0;
+ if (jj >= previous.width) jj = previous.width - 1;
+ if (!isFinite (previous.at (jj,ii)))
+ continue;
+ if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
{
- weight = 1.f/weight;
- r*= weight; g*= weight; b*= weight; a*= weight;
- next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight;
- next.at (j,i).b = static_cast<std::uint8_t> (b);
- next.at (j,i).g = static_cast<std::uint8_t> (g);
- next.at (j,i).r = static_cast<std::uint8_t> (r);
- next.at (j,i).a = static_cast<std::uint8_t> (a);
+ next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
+ next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
+ next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
+ b += previous.at (jj,ii).b * kernel_ (mm,nn);
+ g += previous.at (jj,ii).g * kernel_ (mm,nn);
+ r += previous.at (jj,ii).r * kernel_ (mm,nn);
+ a += previous.at (jj,ii).a * kernel_ (mm,nn);
+ weight+= kernel_ (mm,nn);
}
}
}
+ if (weight == 0)
+ nullify (next.at (j,i));
+ else
+ {
+ weight = 1.f/weight;
+ r*= weight; g*= weight; b*= weight; a*= weight;
+ next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight;
+ next.at (j,i).b = static_cast<std::uint8_t> (b);
+ next.at (j,i).g = static_cast<std::uint8_t> (g);
+ next.at (j,i).r = static_cast<std::uint8_t> (r);
+ next.at (j,i).a = static_cast<std::uint8_t> (a);
+ }
}
- }
+ }
}
+ }
+}
- template<> void
-
Pyramid<pcl::RGB>::nullify (pcl::RGB& p)
- {
- p.r = 0; p.g = 0; p.b = 0;
- }
+template<> void
+Pyramid<pcl::RGB>::nullify (pcl::RGB& p)
+{
+ p.r = 0; p.g = 0; p.b = 0;
+}
- template <> void
- Pyramid<pcl::RGB>::compute (std::vector<Pyramid<pcl::RGB>::PointCloudPtr> &output)
- {
- std::cout << "RGB" << std::endl;
- if (!initCompute ())
- {
- PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
- return;
- }
-
- int kernel_rows = static_cast<int> (kernel_.rows ());
- int kernel_cols = static_cast<int> (kernel_.cols ());
- int kernel_center_x = kernel_cols / 2;
- int kernel_center_y = kernel_rows / 2;
+template <> void
+Pyramid<pcl::RGB>::compute (std::vector<Pyramid<pcl::RGB>::PointCloudPtr> &output)
+{
+ std::cout << "RGB" << std::endl;
+ if (!initCompute ())
+ {
+ PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
+ return;
+ }
- output.resize (levels_ + 1);
- output[0].reset (new pcl::PointCloud<pcl::RGB>);
- *(output[0]) = *input_;
+ int kernel_rows = static_cast<int> (kernel_.rows ());
+ int kernel_cols = static_cast<int> (kernel_.cols ());
+ int kernel_center_x = kernel_cols / 2;
+ int kernel_center_y = kernel_rows / 2;
- if (input_->is_dense)
+ output.resize (levels_ + 1);
+ output[0].reset (new pcl::PointCloud<pcl::RGB>);
+ *(output[0]) = *input_;
+
+ if (input_->is_dense)
+ {
+ for (int l = 1; l <= levels_; ++l)
+ {
+ output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
+ const PointCloud<pcl::RGB> &previous = *output[l-1];
+ PointCloud<pcl::RGB> &next = *output[l];
+#pragma omp parallel for \
+ default(none) \
+ shared(next) \
+ num_threads(threads_)
+ for(int i=0; i < next.height; ++i)
{
- for (int l = 1; l <= levels_; ++l)
+ for(int j=0; j < next.width; ++j)
{
- output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
- const PointCloud<pcl::RGB> &previous = *output[l-1];
- PointCloud<pcl::RGB> &next = *output[l];
-#ifdef _OPENMP
-#pragma omp parallel for shared (next) num_threads(threads_)
-#endif
- for(int i=0; i < next.height; ++i)
+ float r = 0, g = 0, b = 0;
+ for(int m=0; m < kernel_rows; ++m)
{
- for(int j=0; j < next.width; ++j)
+ int mm = kernel_rows - 1 - m;
+ for(int n=0; n < kernel_cols; ++n)
{
- float r = 0, g = 0, b = 0;
- for(int m=0; m < kernel_rows; ++m)
- {
- int mm = kernel_rows - 1 - m;
- for(int n=0; n < kernel_cols; ++n)
- {
- int nn = kernel_cols - 1 - n;
- int ii = 2*i + (m - kernel_center_y);
- int jj = 2*j + (n - kernel_center_x);
- if (ii < 0) ii = 0;
- if (ii >= previous.height) ii = previous.height - 1;
- if (jj < 0) jj = 0;
- if (jj >= previous.width) jj = previous.width - 1;
- b += previous.at (jj,ii).b * kernel_ (mm,nn);
- g += previous.at (jj,ii).g * kernel_ (mm,nn);
- r += previous.at (jj,ii).r * kernel_ (mm,nn);
- }
- }
- next.at (j,i).b = static_cast<std::uint8_t> (b);
- next.at (j,i).g = static_cast<std::uint8_t> (g);
- next.at (j,i).r = static_cast<std::uint8_t> (r);
+ int nn = kernel_cols - 1 - n;
+ int ii = 2*i + (m - kernel_center_y);
+ int jj = 2*j + (n - kernel_center_x);
+ if (ii < 0) ii = 0;
+ if (ii >= previous.height) ii = previous.height - 1;
+ if (jj < 0) jj = 0;
+ if (jj >= previous.width) jj = previous.width - 1;
+ b += previous.at (jj,ii).b * kernel_ (mm,nn);
+ g += previous.at (jj,ii).g * kernel_ (mm,nn);
+ r += previous.at (jj,ii).r * kernel_ (mm,nn);
}
}
+ next.at (j,i).b = static_cast<std::uint8_t> (b);
+ next.at (j,i).g = static_cast<std::uint8_t> (g);
+ next.at (j,i).r = static_cast<std::uint8_t> (r);
}
}
- else
+ }
+ }
+ else
+ {
+ for (int l = 1; l <= levels_; ++l)
+ {
+ output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
+ const PointCloud<pcl::RGB> &previous = *output[l-1];
+ PointCloud<pcl::RGB> &next = *output[l];
+#pragma omp parallel for \
+ default(none) \
+ shared(next) \
+ num_threads(threads_)
+ for(int i=0; i < next.height; ++i)
{
- for (int l = 1; l <= levels_; ++l)
+ for(int j=0; j < next.width; ++j)
{
- output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
- const PointCloud<pcl::RGB> &previous = *output[l-1];
- PointCloud<pcl::RGB> &next = *output[l];
-#ifdef _OPENMP
-#pragma omp parallel for shared (next) num_threads(threads_)
-#endif
- for(int i=0; i < next.height; ++i)
+ float weight = 0;
+ float r = 0, g = 0, b = 0;
+ for(int m=0; m < kernel_rows; ++m)
{
- for(int j=0; j < next.width; ++j)
+ int mm = kernel_rows - 1 - m;
+ for(int n=0; n < kernel_cols; ++n)
{
- float weight = 0;
- float r = 0, g = 0, b = 0;
- for(int m=0; m < kernel_rows; ++m)
- {
- int mm = kernel_rows - 1 - m;
- for(int n=0; n < kernel_cols; ++n)
- {
- int nn = kernel_cols - 1 - n;
- int ii = 2*i + (m - kernel_center_y);
- int jj = 2*j + (n - kernel_center_x);
- if (ii < 0) ii = 0;
- if (ii >= previous.height) ii = previous.height - 1;
- if (jj < 0) jj = 0;
- if (jj >= previous.width) jj = previous.width - 1;
- if (!isFinite (previous.at (jj,ii)))
- continue;
- if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
- {
- b += previous.at (jj,ii).b * kernel_ (mm,nn);
- g += previous.at (jj,ii).g * kernel_ (mm,nn);
- r += previous.at (jj,ii).r * kernel_ (mm,nn);
- weight+= kernel_ (mm,nn);
- }
- }
- }
- if (weight == 0)
- nullify (next.at (j,i));
- else
+ int nn = kernel_cols - 1 - n;
+ int ii = 2*i + (m - kernel_center_y);
+ int jj = 2*j + (n - kernel_center_x);
+ if (ii < 0) ii = 0;
+ if (ii >= previous.height) ii = previous.height - 1;
+ if (jj < 0) jj = 0;
+ if (jj >= previous.width) jj = previous.width - 1;
+ if (!isFinite (previous.at (jj,ii)))
+ continue;
+ if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
{
- weight = 1.f/weight;
- r*= weight; g*= weight; b*= weight;
- next.at (j,i).b = static_cast<std::uint8_t> (b);
- next.at (j,i).g = static_cast<std::uint8_t> (g);
- next.at (j,i).r = static_cast<std::uint8_t> (r);
+ b += previous.at (jj,ii).b * kernel_ (mm,nn);
+ g += previous.at (jj,ii).g * kernel_ (mm,nn);
+ r += previous.at (jj,ii).r * kernel_ (mm,nn);
+ weight+= kernel_ (mm,nn);
}
}
}
+ if (weight == 0)
+ nullify (next.at (j,i));
+ else
+ {
+ weight = 1.f/weight;
+ r*= weight; g*= weight; b*= weight;
+ next.at (j,i).b = static_cast<std::uint8_t> (b);
+ next.at (j,i).g = static_cast<std::uint8_t> (g);
+ next.at (j,i).r = static_cast<std::uint8_t> (r);
+ }
}
- }
+ }
}
-
}
}
+} // namespace filters
+} // namespace pcl
+
#endif
+
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/common/io.h>
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::RadiusOutlierRemoval<PointT>::applyFilter (PointCloud &output)
-{
- std::vector<int> indices;
- if (keep_organized_)
- {
- bool temp = extract_removed_indices_;
- extract_removed_indices_ = true;
- applyFilterIndices (indices);
- extract_removed_indices_ = temp;
-
- output = *input_;
- for (const auto ri : *removed_indices_) // ri = removed index
- output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_;
- if (!std::isfinite (user_filter_value_))
- output.is_dense = false;
- }
- else
- {
- applyFilterIndices (indices);
- copyPointCloud (*input_, indices, output);
- }
-}
-
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
#include <pcl/filters/random_sample.h>
#include <pcl/common/io.h>
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
-///////////////////////////////////////////////////////////////////////////////
-template<typename PointT> void
-pcl::RandomSample<PointT>::applyFilter (PointCloud &output)
-{
- std::vector<int> indices;
- if (keep_organized_)
- {
- bool temp = extract_removed_indices_;
- extract_removed_indices_ = true;
- applyFilter (indices);
- extract_removed_indices_ = temp;
- copyPointCloud (*input_, output);
- // Get X, Y, Z fields
- const auto fields = pcl::getFields<PointT> ();
- std::vector<std::size_t> offsets;
- for (const auto &field : fields)
- {
- if (field.name == "x" ||
- field.name == "y" ||
- field.name == "z")
- offsets.push_back (field.offset);
- }
- // For every "removed" point, set the x,y,z fields to user_filter_value_
- const static float user_filter_value = user_filter_value_;
- for (const auto ri : *removed_indices_) // ri = removed index
- {
- std::uint8_t* pt_data = reinterpret_cast<std::uint8_t*> (&output[ri]);
- for (const unsigned long &offset : offsets)
- {
- memcpy (pt_data + offset, &user_filter_value, sizeof (float));
- }
- }
- if (!std::isfinite (user_filter_value_))
- output.is_dense = false;
- }
- else
- {
- output.is_dense = true;
- applyFilter (indices);
- copyPointCloud (*input_, indices, output);
- }
-}
-
///////////////////////////////////////////////////////////////////////////////
template<typename PointT>
void
#include <iostream>
#include <vector>
#include <pcl/common/eigen.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/filters/sampling_surface_normal.h>
///////////////////////////////////////////////////////////////////////////////
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/io.h>
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output)
-{
- std::vector<int> indices;
- if (keep_organized_)
- {
- bool temp = extract_removed_indices_;
- extract_removed_indices_ = true;
- applyFilterIndices (indices);
- extract_removed_indices_ = temp;
-
- output = *input_;
- for (const auto ri : *removed_indices_) // ri = removed index
- output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_;
- if (!std::isfinite (user_filter_value_))
- output.is_dense = false;
- }
- else
- {
- applyFilterIndices (indices);
- copyPointCloud (*input_, indices, output);
- }
-}
-
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
#include <pcl/common/common.h>
#include <pcl/common/io.h>
#include <pcl/filters/voxel_grid.h>
+#include <boost/sort/spreadsort/integer_sort.hpp>
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
unsigned int idx;
unsigned int cloud_point_index;
+ cloud_point_index_idx() = default;
cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }
};
// Second pass: sort the index_vector vector using value representing target cell as index
// in effect all points belonging to the same output cell will be next to each other
- std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
-
+ auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
+ boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
+
// Third pass: count output cells
// we need to skip all the same, adjacent idx values
unsigned int total = 0;
*/
inline
ModelOutlierRemoval (bool extract_removed_indices = false) :
- FilterIndices<PointT>::FilterIndices (extract_removed_indices)
+ FilterIndices<PointT> (extract_removed_indices)
{
thresh_ = 0;
normals_distance_weight_ = 0;
using FilterIndices<PointT>::extract_removed_indices_;
using FilterIndices<PointT>::removed_indices_;
- /** \brief Filtered results are stored in a separate point cloud.
- * \param[out] output The resultant point cloud.
- */
- void
- applyFilter (PointCloud &output) override;
-
/** \brief Filtered results are indexed by an indices array.
* \param[out] indices The resultant indices.
*/
#pragma once
+#include <pcl/pcl_macros.h>
+#include <pcl/common/utils.h>
#include <pcl/filters/filter.h>
namespace pcl
* \ingroup filters
*/
template <typename NormalT> inline std::vector<float>
- assignNormalWeights (const PointCloud<NormalT>&,
- int,
+ assignNormalWeights (const PointCloud<NormalT>& cloud,
+ int index,
const std::vector<int>& k_indices,
const std::vector<float>& k_sqr_distances)
{
+ pcl::utils::ignore(cloud);
+ pcl::utils::ignore(index);
// Check inputs
if (k_indices.size () != k_sqr_distances.size ())
PCL_ERROR("[pcl::assignNormalWeights] inequal size of neighbor indices and distances!\n");
#include <pcl/filters/boost.h>
#include <pcl/filters/filter_indices.h>
+
#include <ctime>
#include <climits>
+#include <random> // std::mt19937
namespace pcl
{
, binsy_ ()
, binsz_ ()
, input_normals_ ()
- , rng_uniform_distribution_ (nullptr)
{
filter_name_ = "NormalSpaceSampling";
}
- /** \brief Destructor. */
- ~NormalSpaceSampling ()
- {
- delete rng_uniform_distribution_;
- }
-
/** \brief Set number of indices to be sampled.
* \param[in] sample the number of sample indices
*/
/** \brief The normals computed at each point in the input cloud */
NormalsConstPtr input_normals_;
- /** \brief Sample of point indices into a separate PointCloud
- * \param[out] output the resultant point cloud
- */
- void
- applyFilter (PointCloud &output) override;
-
/** \brief Sample of point indices
* \param[out] indices the resultant point cloud indices
*/
private:
/** \brief Finds the bin number of the input normal, returns the bin number
* \param[in] normal the input normal
- * \param[in] nbins total number of bins
*/
unsigned int
- findBin (const float *normal, unsigned int nbins);
+ findBin (const float *normal);
/** \brief Checks of the entire bin is sampled, returns true or false
* \param[out] array flag which says whether a point is sampled or not
bool
isEntireBinSampled (boost::dynamic_bitset<> &array, unsigned int start_index, unsigned int length);
- /** \brief Uniform random distribution. */
- boost::variate_generator<boost::mt19937, boost::uniform_int<std::uint32_t> > *rng_uniform_distribution_;
+ /** \brief Random engine */
+ std::mt19937 rng_;
};
}
* \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false).
*/
PassThrough (bool extract_removed_indices = false) :
- FilterIndices<PointT>::FilterIndices (extract_removed_indices),
+ FilterIndices<PointT> (extract_removed_indices),
filter_field_name_ (""),
filter_limit_min_ (FLT_MIN),
filter_limit_max_ (FLT_MAX)
* \warning This method will be removed in the future. Use setNegative() instead.
* \param[in] limit_negative return data inside the interval (false) or outside (true)
*/
+ PCL_DEPRECATED(1, 13, "use inherited FilterIndices::setNegative() instead")
inline void
setFilterLimitsNegative (const bool limit_negative)
{
* \warning This method will be removed in the future. Use getNegative() instead.
* \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
*/
+ PCL_DEPRECATED(1, 13, "use inherited FilterIndices::getNegative() instead")
inline void
getFilterLimitsNegative (bool &limit_negative) const
{
using FilterIndices<PointT>::extract_removed_indices_;
using FilterIndices<PointT>::removed_indices_;
- /** \brief Filtered results are stored in a separate point cloud.
- * \param[out] output The resultant point cloud.
- */
- void
- applyFilter (PointCloud &output) override;
-
/** \brief Filtered results are indexed by an indices array.
* \param[out] indices The resultant indices.
*/
* Default: false.
* \param[in] limit_negative return data inside the interval (false) or outside (true)
*/
- PCL_DEPRECATED("use inherited FilterIndices::setNegative() instead")
+ PCL_DEPRECATED(1, 12, "use inherited FilterIndices::setNegative() instead")
inline void
setFilterLimitsNegative (const bool limit_negative)
{
/** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
* \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
*/
- PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead")
+ PCL_DEPRECATED(1, 12, "use inherited FilterIndices::getNegative() instead")
inline void
getFilterLimitsNegative (bool &limit_negative) const
{
/** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
* \return true if data \b outside the interval [min; max] is to be returned, false otherwise
*/
- PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead")
+ PCL_DEPRECATED(1, 12, "use inherited FilterIndices::getNegative() instead")
inline bool
getFilterLimitsNegative () const
{
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
-#include <pcl/common/point_operators.h>
#include <pcl/point_cloud.h>
#include <pcl/pcl_config.h>
* \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false).
*/
RadiusOutlierRemoval (bool extract_removed_indices = false) :
- FilterIndices<PointT>::FilterIndices (extract_removed_indices),
+ FilterIndices<PointT> (extract_removed_indices),
searcher_ (),
search_radius_ (0.0),
min_pts_radius_ (1)
using FilterIndices<PointT>::extract_removed_indices_;
using FilterIndices<PointT>::removed_indices_;
- /** \brief Filtered results are stored in a separate point cloud.
- * \param[out] output The resultant point cloud.
- */
- void
- applyFilter (PointCloud &output) override;
-
/** \brief Filtered results are indexed by an indices array.
* \param[out] indices The resultant indices.
*/
/** \brief Random number seed. */
unsigned int seed_;
- /** \brief Sample of point indices into a separate PointCloud
- * \param output the resultant point cloud
- */
- void
- applyFilter (PointCloud &output) override;
-
/** \brief Sample of point indices
* \param indices the resultant point cloud indices
*/
* \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false).
*/
StatisticalOutlierRemoval (bool extract_removed_indices = false) :
- FilterIndices<PointT>::FilterIndices (extract_removed_indices),
+ FilterIndices<PointT> (extract_removed_indices),
searcher_ (),
mean_k_ (1),
std_mul_ (0.0)
using FilterIndices<PointT>::extract_removed_indices_;
using FilterIndices<PointT>::removed_indices_;
- /** \brief Filtered results are stored in a separate point cloud.
- * \param[out] output The resultant point cloud.
- */
- void
- applyFilter (PointCloud &output) override;
-
/** \brief Filtered results are indexed by an indices array.
* \param[out] indices The resultant indices.
*/
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2020-, Open Perception
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/filters/convolution.h>
+
+namespace pcl {
+namespace filters {
+template <>
+pcl::PointXYZRGB
+Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense(int i, int j)
+{
+ pcl::PointXYZRGB result;
+ float r = 0, g = 0, b = 0;
+ for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) {
+ result.x += (*input_)(l, j).x * kernel_[k];
+ result.y += (*input_)(l, j).y * kernel_[k];
+ result.z += (*input_)(l, j).z * kernel_[k];
+ r += kernel_[k] * static_cast<float>((*input_)(l, j).r);
+ g += kernel_[k] * static_cast<float>((*input_)(l, j).g);
+ b += kernel_[k] * static_cast<float>((*input_)(l, j).b);
+ }
+ result.r = static_cast<std::uint8_t>(r);
+ result.g = static_cast<std::uint8_t>(g);
+ result.b = static_cast<std::uint8_t>(b);
+ return (result);
+}
+
+template <>
+pcl::PointXYZRGB
+Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense(int i, int j)
+{
+ pcl::PointXYZRGB result;
+ float r = 0, g = 0, b = 0;
+ for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) {
+ result.x += (*input_)(i, l).x * kernel_[k];
+ result.y += (*input_)(i, l).y * kernel_[k];
+ result.z += (*input_)(i, l).z * kernel_[k];
+ r += kernel_[k] * static_cast<float>((*input_)(i, l).r);
+ g += kernel_[k] * static_cast<float>((*input_)(i, l).g);
+ b += kernel_[k] * static_cast<float>((*input_)(i, l).b);
+ }
+ result.r = static_cast<std::uint8_t>(r);
+ result.g = static_cast<std::uint8_t>(g);
+ result.b = static_cast<std::uint8_t>(b);
+ return (result);
+}
+
+template <>
+pcl::PointXYZRGB
+Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense(int i, int j)
+{
+ pcl::PointXYZRGB result;
+ float weight = 0;
+ float r = 0, g = 0, b = 0;
+ for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) {
+ if (!isFinite((*input_)(l, j)))
+ continue;
+ if (pcl::squaredEuclideanDistance((*input_)(i, j), (*input_)(l, j)) <
+ distance_threshold_) {
+ result.x += (*input_)(l, j).x * kernel_[k];
+ result.y += (*input_)(l, j).y * kernel_[k];
+ result.z += (*input_)(l, j).z * kernel_[k];
+ r += kernel_[k] * static_cast<float>((*input_)(l, j).r);
+ g += kernel_[k] * static_cast<float>((*input_)(l, j).g);
+ b += kernel_[k] * static_cast<float>((*input_)(l, j).b);
+ weight += kernel_[k];
+ }
+ }
+
+ if (weight == 0)
+ result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN();
+ else {
+ weight = 1.f / weight;
+ r *= weight;
+ g *= weight;
+ b *= weight;
+ result.x *= weight;
+ result.y *= weight;
+ result.z *= weight;
+ result.r = static_cast<std::uint8_t>(r);
+ result.g = static_cast<std::uint8_t>(g);
+ result.b = static_cast<std::uint8_t>(b);
+ }
+ return (result);
+}
+
+template <>
+pcl::PointXYZRGB
+Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense(int i, int j)
+{
+ pcl::PointXYZRGB result;
+ float weight = 0;
+ float r = 0, g = 0, b = 0;
+ for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) {
+ if (!isFinite((*input_)(i, l)))
+ continue;
+ if (pcl::squaredEuclideanDistance((*input_)(i, j), (*input_)(i, l)) <
+ distance_threshold_) {
+ result.x += (*input_)(i, l).x * kernel_[k];
+ result.y += (*input_)(i, l).y * kernel_[k];
+ result.z += (*input_)(i, l).z * kernel_[k];
+ r += kernel_[k] * static_cast<float>((*input_)(i, l).r);
+ g += kernel_[k] * static_cast<float>((*input_)(i, l).g);
+ b += kernel_[k] * static_cast<float>((*input_)(i, l).b);
+ weight += kernel_[k];
+ }
+ }
+ if (weight == 0)
+ result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN();
+ else {
+ weight = 1.f / weight;
+ r *= weight;
+ g *= weight;
+ b *= weight;
+ result.x *= weight;
+ result.y *= weight;
+ result.z *= weight;
+ result.r = static_cast<std::uint8_t>(r);
+ result.g = static_cast<std::uint8_t>(g);
+ result.b = static_cast<std::uint8_t>(b);
+ }
+ return (result);
+}
+
+template <>
+pcl::RGB
+Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense(int i, int j)
+{
+ pcl::RGB result;
+ float r = 0, g = 0, b = 0;
+ for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) {
+ r += kernel_[k] * static_cast<float>((*input_)(l, j).r);
+ g += kernel_[k] * static_cast<float>((*input_)(l, j).g);
+ b += kernel_[k] * static_cast<float>((*input_)(l, j).b);
+ }
+ result.r = static_cast<std::uint8_t>(r);
+ result.g = static_cast<std::uint8_t>(g);
+ result.b = static_cast<std::uint8_t>(b);
+ return (result);
+}
+
+template <>
+pcl::RGB
+Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense(int i, int j)
+{
+ pcl::RGB result;
+ float r = 0, g = 0, b = 0;
+ for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) {
+ r += kernel_[k] * static_cast<float>((*input_)(i, l).r);
+ g += kernel_[k] * static_cast<float>((*input_)(i, l).g);
+ b += kernel_[k] * static_cast<float>((*input_)(i, l).b);
+ }
+ result.r = static_cast<std::uint8_t>(r);
+ result.g = static_cast<std::uint8_t>(g);
+ result.b = static_cast<std::uint8_t>(b);
+ return (result);
+}
+} // namespace filters
+} // namespace pcl
// Instantiations of specific point types
PCL_INSTANTIATE(removeNanFromPointCloud, PCL_XYZ_POINT_TYPES)
+PCL_INSTANTIATE(FilterIndices, PCL_POINT_TYPES)
#endif // PCL_NO_PRECOMPILE
#include <pcl/filters/impl/radius_outlier_removal.hpp>
#include <pcl/conversions.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
///////////////////////////////////////////////////////////////////////////////////////////
void
#include <iostream>
#include <pcl/common/io.h>
#include <pcl/filters/impl/voxel_grid.hpp>
+#include <boost/sort/spreadsort/integer_sort.hpp>
using Array4size_t = Eigen::Array<std::size_t, 4, 1>;
// Second pass: sort the index_vector vector using value representing target cell as index
// in effect all points belonging to the same output cell will be next to each other
- std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
+ auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
+ boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
// Third pass: count output cells
// we need to skip all the same, adjacenent idx values
PCL_INSTANTIATE(VoxelGrid, PCL_XYZ_POINT_TYPES)
#endif // PCL_NO_PRECOMPILE
-
The PCL geometry library contains computational geometry data structures and algorithms.
- \section secRecognitionRequirements Requirements
+ \section secGeometryRequirements Requirements
- \ref common "common"
*/
#endif
#include <boost/operators.hpp>
-#include <boost/shared_ptr.hpp>
#include <boost/version.hpp>
#include <pcl/geometry/mesh_indices.h>
#include <pcl/geometry/mesh_elements.h>
#include <pcl/geometry/mesh_traits.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#pragma once
#include <pcl/common/eigen.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/ModelCoefficients.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/geometry/mesh_base.h>
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
- * $Id: point_operators.h 4389 2012-02-10 10:10:26Z nizar $
- *
*/
#pragma once
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/geometry/mesh_base.h>
#include <utility>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/geometry/mesh_base.h>
sort_relative(PCL_GPU_MODULES_NAMES_UNSORTED PCL_GPU_MODULES_NAMES PCL_GPU_MODULES_DIRS)
foreach(subdir ${PCL_GPU_MODULES_DIRS})
add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/${subdir}")
-endforeach()
\ No newline at end of file
+endforeach()
* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
*/
+
#ifndef PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_
#define PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_
+namespace pcl
+{
+
+namespace gpu
+{
+
///////////////////// Inline implementations of DeviceArray ////////////////////////////////////////////
-template<class T> inline pcl::gpu::DeviceArray<T>::DeviceArray() {}
-template<class T> inline pcl::gpu::DeviceArray<T>::DeviceArray(std::size_t size) : DeviceMemory(size * elem_size) {}
-template<class T> inline pcl::gpu::DeviceArray<T>::DeviceArray(T *ptr, std::size_t size) : DeviceMemory(ptr, size * elem_size) {}
-template<class T> inline pcl::gpu::DeviceArray<T>::DeviceArray(const DeviceArray& other) : DeviceMemory(other) {}
-template<class T> inline pcl::gpu::DeviceArray<T>& pcl::gpu::DeviceArray<T>::operator=(const DeviceArray& other)
+template<class T> inline DeviceArray<T>::DeviceArray() {}
+template<class T> inline DeviceArray<T>::DeviceArray(std::size_t size) : DeviceMemory(size * elem_size) {}
+template<class T> inline DeviceArray<T>::DeviceArray(T *ptr, std::size_t size) : DeviceMemory(ptr, size * elem_size) {}
+template<class T> inline DeviceArray<T>::DeviceArray(const DeviceArray& other) : DeviceMemory(other) {}
+template<class T> inline DeviceArray<T>& DeviceArray<T>::operator=(const DeviceArray& other)
{ DeviceMemory::operator=(other); return *this; }
-template<class T> inline void pcl::gpu::DeviceArray<T>::create(std::size_t size)
+template<class T> inline void DeviceArray<T>::create(std::size_t size)
{ DeviceMemory::create(size * elem_size); }
-template<class T> inline void pcl::gpu::DeviceArray<T>::release()
+template<class T> inline void DeviceArray<T>::release()
{ DeviceMemory::release(); }
-template<class T> inline void pcl::gpu::DeviceArray<T>::copyTo(DeviceArray& other) const
+template<class T> inline void DeviceArray<T>::copyTo(DeviceArray& other) const
{ DeviceMemory::copyTo(other); }
-template<class T> inline void pcl::gpu::DeviceArray<T>::upload(const T *host_ptr, std::size_t size)
+template<class T> inline void DeviceArray<T>::upload(const T *host_ptr, std::size_t size)
{ DeviceMemory::upload(host_ptr, size * elem_size); }
-template<class T> inline void pcl::gpu::DeviceArray<T>::download(T *host_ptr) const
+template<class T> inline void DeviceArray<T>::download(T *host_ptr) const
{ DeviceMemory::download( host_ptr ); }
-template<class T> void pcl::gpu::DeviceArray<T>::swap(DeviceArray& other_arg) { DeviceMemory::swap(other_arg); }
+template<class T> void DeviceArray<T>::swap(DeviceArray& other_arg) { DeviceMemory::swap(other_arg); }
-template<class T> inline pcl::gpu::DeviceArray<T>::operator T*() { return ptr(); }
-template<class T> inline pcl::gpu::DeviceArray<T>::operator const T*() const { return ptr(); }
-template<class T> inline std::size_t pcl::gpu::DeviceArray<T>::size() const { return sizeBytes() / elem_size; }
+template<class T> inline DeviceArray<T>::operator T*() { return ptr(); }
+template<class T> inline DeviceArray<T>::operator const T*() const { return ptr(); }
+template<class T> inline std::size_t DeviceArray<T>::size() const { return sizeBytes() / elem_size; }
-template<class T> inline T* pcl::gpu::DeviceArray<T>::ptr() { return DeviceMemory::ptr<T>(); }
-template<class T> inline const T* pcl::gpu::DeviceArray<T>::ptr() const { return DeviceMemory::ptr<T>(); }
+template<class T> inline T* DeviceArray<T>::ptr() { return DeviceMemory::ptr<T>(); }
+template<class T> inline const T* DeviceArray<T>::ptr() const { return DeviceMemory::ptr<T>(); }
-template<class T> template<class A> inline void pcl::gpu::DeviceArray<T>::upload(const std::vector<T, A>& data) { upload(&data[0], data.size()); }
-template<class T> template<class A> inline void pcl::gpu::DeviceArray<T>::download(std::vector<T, A>& data) const { data.resize(size()); if (!data.empty()) download(&data[0]); }
+template<class T> template<class A> inline void DeviceArray<T>::upload(const std::vector<T, A>& data) { upload(&data[0], data.size()); }
+template<class T> template<class A> inline void DeviceArray<T>::download(std::vector<T, A>& data) const { data.resize(size()); if (!data.empty()) download(&data[0]); }
///////////////////// Inline implementations of DeviceArray2D ////////////////////////////////////////////
-template<class T> inline pcl::gpu::DeviceArray2D<T>::DeviceArray2D() {}
-template<class T> inline pcl::gpu::DeviceArray2D<T>::DeviceArray2D(int rows, int cols) : DeviceMemory2D(rows, cols * elem_size) {}
-template<class T> inline pcl::gpu::DeviceArray2D<T>::DeviceArray2D(int rows, int cols, void *data, std::size_t stepBytes) : DeviceMemory2D(rows, cols * elem_size, data, stepBytes) {}
-template<class T> inline pcl::gpu::DeviceArray2D<T>::DeviceArray2D(const DeviceArray2D& other) : DeviceMemory2D(other) {}
-template<class T> inline pcl::gpu::DeviceArray2D<T>& pcl::gpu::DeviceArray2D<T>::operator=(const DeviceArray2D& other)
+template<class T> inline DeviceArray2D<T>::DeviceArray2D() {}
+template<class T> inline DeviceArray2D<T>::DeviceArray2D(int rows, int cols) : DeviceMemory2D(rows, cols * elem_size) {}
+template<class T> inline DeviceArray2D<T>::DeviceArray2D(int rows, int cols, void *data, std::size_t stepBytes) : DeviceMemory2D(rows, cols * elem_size, data, stepBytes) {}
+template<class T> inline DeviceArray2D<T>::DeviceArray2D(const DeviceArray2D& other) : DeviceMemory2D(other) {}
+template<class T> inline DeviceArray2D<T>& DeviceArray2D<T>::operator=(const DeviceArray2D& other)
{ DeviceMemory2D::operator=(other); return *this; }
-template<class T> inline void pcl::gpu::DeviceArray2D<T>::create(int rows, int cols)
+template<class T> inline void DeviceArray2D<T>::create(int rows, int cols)
{ DeviceMemory2D::create(rows, cols * elem_size); }
-template<class T> inline void pcl::gpu::DeviceArray2D<T>::release()
+template<class T> inline void DeviceArray2D<T>::release()
{ DeviceMemory2D::release(); }
-template<class T> inline void pcl::gpu::DeviceArray2D<T>::copyTo(DeviceArray2D& other) const
+template<class T> inline void DeviceArray2D<T>::copyTo(DeviceArray2D& other) const
{ DeviceMemory2D::copyTo(other); }
-template<class T> inline void pcl::gpu::DeviceArray2D<T>::upload(const void *host_ptr, std::size_t host_step, int rows, int cols)
+template<class T> inline void DeviceArray2D<T>::upload(const void *host_ptr, std::size_t host_step, int rows, int cols)
{ DeviceMemory2D::upload(host_ptr, host_step, rows, cols * elem_size); }
-template<class T> inline void pcl::gpu::DeviceArray2D<T>::download(void *host_ptr, std::size_t host_step) const
+template<class T> inline void DeviceArray2D<T>::download(void *host_ptr, std::size_t host_step) const
{ DeviceMemory2D::download( host_ptr, host_step ); }
-template<class T> template<class A> inline void pcl::gpu::DeviceArray2D<T>::upload(const std::vector<T, A>& data, int cols)
+template<class T> template<class A> inline void DeviceArray2D<T>::upload(const std::vector<T, A>& data, int cols)
{ upload(&data[0], cols * elem_size, data.size()/cols, cols); }
-template<class T> template<class A> inline void pcl::gpu::DeviceArray2D<T>::download(std::vector<T, A>& data, int& elem_step) const
+template<class T> template<class A> inline void DeviceArray2D<T>::download(std::vector<T, A>& data, int& elem_step) const
{ elem_step = cols(); data.resize(cols() * rows()); if (!data.empty()) download(&data[0], colsBytes()); }
-template<class T> void pcl::gpu::DeviceArray2D<T>::swap(DeviceArray2D& other_arg) { DeviceMemory2D::swap(other_arg); }
+template<class T> void DeviceArray2D<T>::swap(DeviceArray2D& other_arg) { DeviceMemory2D::swap(other_arg); }
+
+template<class T> inline T* DeviceArray2D<T>::ptr(int y) { return DeviceMemory2D::ptr<T>(y); }
+template<class T> inline const T* DeviceArray2D<T>::ptr(int y) const { return DeviceMemory2D::ptr<T>(y); }
+
+template<class T> inline DeviceArray2D<T>::operator T*() { return ptr(); }
+template<class T> inline DeviceArray2D<T>::operator const T*() const { return ptr(); }
-template<class T> inline T* pcl::gpu::DeviceArray2D<T>::ptr(int y) { return DeviceMemory2D::ptr<T>(y); }
-template<class T> inline const T* pcl::gpu::DeviceArray2D<T>::ptr(int y) const { return DeviceMemory2D::ptr<T>(y); }
-
-template<class T> inline pcl::gpu::DeviceArray2D<T>::operator T*() { return ptr(); }
-template<class T> inline pcl::gpu::DeviceArray2D<T>::operator const T*() const { return ptr(); }
+template<class T> inline int DeviceArray2D<T>::cols() const { return DeviceMemory2D::colsBytes()/elem_size; }
+template<class T> inline int DeviceArray2D<T>::rows() const { return DeviceMemory2D::rows(); }
-template<class T> inline int pcl::gpu::DeviceArray2D<T>::cols() const { return DeviceMemory2D::colsBytes()/elem_size; }
-template<class T> inline int pcl::gpu::DeviceArray2D<T>::rows() const { return DeviceMemory2D::rows(); }
+template<class T> inline std::size_t DeviceArray2D<T>::elem_step() const { return DeviceMemory2D::step()/elem_size; }
-template<class T> inline std::size_t pcl::gpu::DeviceArray2D<T>::elem_step() const { return DeviceMemory2D::step()/elem_size; }
+} // namespace gpu
+} // namespace pcl
+#endif /* PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_ */
-#endif /* PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_ */
#ifndef PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_
#define PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_
+namespace pcl
+{
+
+namespace gpu
+{
+
///////////////////// Inline implementations of DeviceMemory ////////////////////////////////////////////
+template<class T> inline T*
+DeviceMemory::ptr()
+{
+ return (T*)data_;
+}
+
+template<class T> inline const T*
+DeviceMemory::ptr() const
+{
+ return (const T*)data_;
+}
-template<class T> inline T* pcl::gpu::DeviceMemory::ptr() { return ( T*)data_; }
-template<class T> inline const T* pcl::gpu::DeviceMemory::ptr() const { return (const T*)data_; }
-
-template <class U> inline pcl::gpu::DeviceMemory::operator pcl::gpu::PtrSz<U>() const
+template <class U> inline DeviceMemory::operator
+PtrSz<U>() const
{
PtrSz<U> result;
result.data = (U*)ptr<U>();
result.size = sizeBytes_/sizeof(U);
- return result;
+ return result;
}
///////////////////// Inline implementations of DeviceMemory2D ////////////////////////////////////////////
-
-template<class T> T* pcl::gpu::DeviceMemory2D::ptr(int y_arg) { return ( T*)(( char*)data_ + y_arg * step_); }
-template<class T> const T* pcl::gpu::DeviceMemory2D::ptr(int y_arg) const { return (const T*)((const char*)data_ + y_arg * step_); }
-
-template <class U> pcl::gpu::DeviceMemory2D::operator pcl::gpu::PtrStep<U>() const
+template<class T> T*
+DeviceMemory2D::ptr(int y_arg)
+{
+ return (T*)((char*)data_ + y_arg * step_);
+}
+
+template<class T> const T*
+DeviceMemory2D::ptr(int y_arg) const
+{
+ return (const T*)((const char*)data_ + y_arg * step_);
+}
+
+template <class U> DeviceMemory2D::operator
+PtrStep<U>() const
{
PtrStep<U> result;
result.data = (U*)ptr<U>();
return result;
}
-template <class U> pcl::gpu::DeviceMemory2D::operator pcl::gpu::PtrStepSz<U>() const
+template <class U> DeviceMemory2D::operator
+PtrStepSz<U>() const
{
PtrStepSz<U> result;
result.data = (U*)ptr<U>();
return result;
}
-#endif /* PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_ */
+} // namespace gpu
+} // namespace pcl
+#endif /* PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_ */
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
+#include <pcl/memory.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
-#include <boost/shared_ptr.hpp>
#include <pcl/visualization/cloud_viewer.h>
+
#include <iostream>
int main (int argc, char** argv)
NormalEstimation();
void compute(Normals& normals);
void setViewPoint(float vpx, float vpy, float vpz);
- void getViewPoint(float& vpx, float& vpy, float& vpz);
+ void getViewPoint(float& vpx, float& vpy, float& vpz) const;
static void computeNormals(const PointCloud& cloud, const NeighborIndices& nn_indices, Normals& normals);
static void flipNormalTowardsViewpoint(const PointCloud& cloud, float vp_x, float vp_y, float vp_z, Normals& normals);
VFHEstimation();
void setViewPoint(float vpx, float vpy, float vpz);
- void getViewPoint(float& vpx, float& vpy, float& vpz);
+ void getViewPoint(float& vpx, float& vpy, float& vpz) const;
void setUseGivenNormal (bool use);
void setNormalToUse (const NormalType& normal);
vpx_ = vpx; vpy_ = vpy; vpz_ = vpz;
}
-void pcl::gpu::NormalEstimation::getViewPoint (float &vpx, float &vpy, float &vpz)
+void pcl::gpu::NormalEstimation::getViewPoint (float &vpx, float &vpy, float &vpz) const
{
vpx = vpx_; vpy = vpy_; vpz = vpz_;
}
}
void pcl::gpu::VFHEstimation::setViewPoint(float vpx, float vpy, float vpz) { vpx_ = vpx; vpy_ = vpy; vpz_ = vpz; }
-void pcl::gpu::VFHEstimation::getViewPoint(float& vpx, float& vpy, float& vpz) { vpx = vpx_; vpy = vpy_; vpz = vpz_; }
+void pcl::gpu::VFHEstimation::getViewPoint(float& vpx, float& vpy, float& vpz) const { vpx = vpx_; vpy = vpy_; vpz = vpz_; }
void pcl::gpu::VFHEstimation::setUseGivenNormal (bool use) { use_given_normal_ = use; }
void pcl::gpu::VFHEstimation::setNormalToUse (const NormalType& normal) { normal_to_use_ = normal; }
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/point_types.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/gpu/kinfu/pixel_rgb.h>
* \param[out] cy principal point y
*/
void
- getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy);
+ getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy) const;
/** \brief Sets initial camera pose relative to volume coordinate space
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
#include <Eigen/Core>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/gpu/kinfu/pixel_rgb.h>
-#include <pcl/make_shared.h>
#include <Eigen/Geometry>
namespace pcl
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/point_types.h>
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::gpu::KinfuTracker::getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy)
+pcl::gpu::KinfuTracker::getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy) const
{
fx = fx_;
fy = fy_;
tsdf.resize (volume_.cols() * volume_.rows());
volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
-#pragma omp parallel for
+#pragma omp parallel for \
+ default(none) \
+ shared(tsdf)
for(int i = 0; i < (int) tsdf.size(); ++i)
{
float tmp = reinterpret_cast<short2*>(&tsdf[i])->x;
weights.resize (volumeSize);
volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
-#pragma omp parallel for
+#pragma omp parallel for \
+ default(none) \
+ shared(weights, tsdf)
for(int i = 0; i < (int) tsdf.size(); ++i)
{
short2 elem = *reinterpret_cast<short2*>(&tsdf[i]);
#include <Eigen/Geometry>
#include <fstream>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
/**
* @brief The CameraPoseProcessor class is an interface to extract
#include <pcl/gpu/containers/kernel_containers.h>
#include <pcl/gpu/kinfu/kinfu.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <memory>
#include <string>
#include "evaluation.h"
#include <pcl/common/angles.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include "tsdf_volume.h"
#include "tsdf_volume.hpp"
}
void
- showDepth (const PtrStepSz<const unsigned short>& depth)
+ showDepth (const PtrStepSz<const unsigned short>& depth) const
{
if (viz_)
viewerDepth_->showShortImage (depth.data, depth.cols, depth.rows, 0, 5000, true, "short_image");
}
void
- clearClouds (bool print_message = false)
+ clearClouds (bool print_message = false) const
{
if (!viz_)
return;
#include "evaluation.h"
#include <pcl/common/angles.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include "tsdf_volume.h"
#include "tsdf_volume.hpp"
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef _WIN32
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
#include "pcl/common/common.h"
#include "pcl/common/transforms.h"
#include <pcl/console/print.h>
-// define the following in order to eliminate the deprecated headers warning
-#define VTK_EXCLUDE_STRSTREAM_HEADERS
#include <pcl/io/vtk_lib_io.h>
//
#include <pcl/simulation/camera.h>
device_volume_.download (&volume_vec[0], device_volume_.cols() * sizeof(int));
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none) \
+ shared(volume, volume_vec, weights_vec)
for(int i = 0; i < (int) volume->size(); ++i)
{
short2 *elem = (short2*)&volume_vec[i];
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
// Functionality
/** \brief Converts volume to cloud of TSDF values
- * \param[ou] cloud - the output point cloud
+ * \param[out] cloud - the output point cloud
* \param[in] step - the decimation step to use
*/
void
cloud->reserve (std::min (cloud_size/10, 500000));
int volume_idx = 0, cloud_idx = 0;
-//#pragma omp parallel for // if used, increment over idx not possible! use index calculation
for (int z = 0; z < sz; z+=step)
for (int y = 0; y < sy; y+=step)
for (int x = 0; x < sx; x+=step, ++cloud_idx)
const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
// loop over all voxels in 3D neighborhood
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none)
for (int z = min_index(2); z <= max_index(2); ++z)
{
for (int y = min_index(1); y <= max_index(1); ++y)
Eigen::Vector3i index = min_index;
// loop over all voxels in 3D neighborhood
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none)
for (int z = min_index(2); z <= max_index(2); ++z)
{
for (int y = min_index(1); y <= max_index(1); ++y)
template <typename VoxelT, typename WeightT> void
pcl::TSDFVolume<VoxelT, WeightT>::averageValues ()
{
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none)
for (std::size_t i = 0; i < volume_->size(); ++i)
{
WeightT &w = weights_->at(i);
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/point_types.h>
#define PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
#include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
///////////////////////////////////////////////////////////////////////////////
template <typename PointT>
constexpr int DIVISOR = std::numeric_limits<short>::max();
///For every point in the cloud
-#pragma omp parallel for
-
+#pragma omp parallel for \
+ default(none) \
+ shared(cloud, output)
for(int i = 0; i < (int) cloud.points.size (); ++i)
{
int x = cloud.points[i].x;
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
#include <Eigen/Core>
#include <iostream>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/gpu/kinfu_large_scale/pixel_rgb.h>
-#include <boost/shared_ptr.hpp>
//#include <boost/graph/buffer_concepts.hpp>
#include <Eigen/Geometry>
/**Write camera pose to file*/
void
- writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> &erreMats);
+ writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> &erreMats) const;
/**Counter of the number of screenshots taken*/
int screenshot_counter;
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/point_types.h>
}
/** \brief Converts volume stored on host to cloud of TSDF values
- * \param[ou] cloud - the output point cloud
+ * \param[out] cloud - the output point cloud
* \param[in] step - the decimation step to use
*/
void
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
- ScreenshotManager::writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> &erreMats)
+ ScreenshotManager::writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> &erreMats) const
{
std::ofstream poseFile;
poseFile.open (filename_pose.c_str());
cloud->reserve (std::min (cloud_size/10, 500000));
int volume_idx = 0, cloud_idx = 0;
- // #pragma omp parallel for // if used, increment over idx not possible! use index calculation
for (int z = 0; z < sz; z+=step)
for (int y = 0; y < sy; y+=step)
for (int x = 0; x < sx; x+=step, ++cloud_idx)
tsdf.resize (volume_.cols() * volume_.rows());
volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
-#pragma omp parallel for
+#pragma omp parallel for \
+ default(none) \
+ shared(tsdf)
for(int i = 0; i < (int) tsdf.size(); ++i)
{
float tmp = reinterpret_cast<short2*>(&tsdf[i])->x;
weights.resize (volumeSize);
volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none) \
+ shared(tsdf, weights)
for(int i = 0; i < (int) tsdf.size(); ++i)
{
short2 elem = *reinterpret_cast<short2*>(&tsdf[i]);
#include <pcl/gpu/containers/kernel_containers.h>
#include <pcl/gpu/kinfu_large_scale/kinfu.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <memory>
#include <string>
#include <iostream>
#include <thread>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/console/parse.h>
#include "evaluation.h"
#include <pcl/common/angles.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENCV
#include <opencv2/highgui/highgui.hpp>
#include "evaluation.h"
#include <pcl/common/angles.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
//#include "tsdf_volume.h"
//#include "tsdf_volume.hpp"
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
-#include <boost/shared_ptr.hpp>
#ifdef _WIN32
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
#include "pcl/common/common.h"
#include "pcl/common/transforms.h"
#include <pcl/console/print.h>
-// define the following in order to eliminate the deprecated headers warning
-#define VTK_EXCLUDE_STRSTREAM_HEADERS
#include <pcl/io/vtk_lib_io.h>
//
#include <pcl/simulation/camera.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h> //fps calculations
device_volume_.download (&volume_vec[0], device_volume_.cols() * sizeof(int));
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none) \
+ shared(volume, volume_vec, weights_vec)
for(int i = 0; i < (int) volume->size(); ++i)
{
short2 *elem = (short2*)&volume_vec[i];
#include <vector>
+#include <pcl/memory.h>
#include <pcl/point_types.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
void build();
/** \brief Returns true if tree has been built */
- bool isBuilt();
+ bool isBuilt() const;
/** \brief Downloads Octree from GPU to search using CPU function. It use useful for single (not-batch) search */
void internalDownload();
void pcl::gpu::Octree::clear()
{
- if (impl)
delete static_cast<OctreeImpl*>(impl);
}
built_ = true;
}
-bool pcl::gpu::Octree::isBuilt()
+bool pcl::gpu::Octree::isBuilt() const
{
return built_;
}
* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
*/
+
#ifndef _PCL_TEST_GPU_OCTREE_DATAGEN_
#define _PCL_TEST_GPU_OCTREE_DATAGEN_
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::PointXYZ)
#endif
+
+namespace pcl
+{
+
+namespace gpu
+{
+
struct DataGenerator
{
- using PointType = pcl::gpu::Octree::PointType;
+ using PointType = Octree::PointType;
- std::size_t data_size;
+ std::size_t data_size;
std::size_t tests_num;
float cube_size;
- float max_radius;
+ float max_radius;
float shared_radius;
}
void operator()()
- {
+ {
srand (0);
points.resize(data_size);
for(std::size_t i = 0; i < data_size; ++i)
- {
- points[i].x = ((float)rand())/RAND_MAX * cube_size;
- points[i].y = ((float)rand())/RAND_MAX * cube_size;
+ {
+ points[i].x = ((float)rand())/RAND_MAX * cube_size;
+ points[i].y = ((float)rand())/RAND_MAX * cube_size;
points[i].z = ((float)rand())/RAND_MAX * cube_size;
}
-
+
queries.resize(tests_num);
radiuses.resize(tests_num);
for (std::size_t i = 0; i < tests_num; ++i)
- {
- queries[i].x = ((float)rand())/RAND_MAX * cube_size;
- queries[i].y = ((float)rand())/RAND_MAX * cube_size;
- queries[i].z = ((float)rand())/RAND_MAX * cube_size;
- radiuses[i] = ((float)rand())/RAND_MAX * max_radius;
- };
+ {
+ queries[i].x = ((float)rand())/RAND_MAX * cube_size;
+ queries[i].y = ((float)rand())/RAND_MAX * cube_size;
+ queries[i].z = ((float)rand())/RAND_MAX * cube_size;
+ radiuses[i] = ((float)rand())/RAND_MAX * max_radius;
+ };
for(std::size_t i = 0; i < tests_num/2; ++i)
indices.push_back(i*2);
}
void bruteForceSearch(bool log = false, float radius = -1.f)
- {
+ {
if (log)
std::cout << "BruteForceSearch";
int value100 = std::min<int>(tests_num, 50);
- int step = tests_num/value100;
+ int step = tests_num/value100;
bfresutls.resize(tests_num);
for(std::size_t i = 0; i < tests_num; ++i)
- {
+ {
if (log && i % step == 0)
{
std::cout << ".";
std::vector<int>& curr_res = bfresutls[i];
curr_res.clear();
-
+
float query_radius = radius > 0 ? radius : radiuses[i];
const PointType& query = queries[i];
std::cout << "Done" << std::endl;
}
- void printParams() const
- {
+ void printParams() const
+ {
std::cout << "Points number = " << data_size << std::endl;
std::cout << "Queries number = " << tests_num << std::endl;
std::cout << "Cube size = " << cube_size << std::endl;
template<typename Dst>
struct ConvPoint
- {
- Dst operator()(const PointType& src) const
+ {
+ Dst operator()(const PointType& src) const
{
Dst dst;
dst.x = src.x;
return dst;
}
};
-
};
-#endif /* _PCL_TEST_GPU_OCTREE_DATAGEN_ */
-
+} // namespace gpu
+} // namespace pcl
+#endif /* _PCL_TEST_GPU_OCTREE_DATAGEN_ */
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/gpu/people/label_blob2.h>
#include <pcl/gpu/people/label_common.h>
#include "pcl/gpu/people/person_attribs.h"
-#include <boost/shared_ptr.hpp>
-#include <memory>
+
#include <string>
#include <vector>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
-#include <boost/shared_ptr.hpp>
+
#include <string>
#include <vector>
/** \brief Get the cuda GPU device id in use **/
int
- getDeviceId() {return cuda_dev_id_;}
+ getDeviceId() const {return cuda_dev_id_;}
private:
bool largest_object_; /** \brief only give back largest object **/
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/gpu/people/label_common.h>
-#include <boost/shared_ptr.hpp>
#include <string>
#include <vector>
#include <string>
#include <vector>
#include <iosfwd>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
return NCV_SUCCESS;
}
- __device__ __host__ NcvBool isTilted()
+ __device__ __host__ NcvBool isTilted() const
{
return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagTilted) != 0;
}
- __device__ __host__ NcvBool isLeftNodeLeaf()
+ __device__ __host__ NcvBool isLeftNodeLeaf() const
{
return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagLeftNodeLeaf) != 0;
}
- __device__ __host__ NcvBool isRightNodeLeaf()
+ __device__ __host__ NcvBool isRightNodeLeaf() const
{
return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagRightNodeLeaf) != 0;
}
- __device__ __host__ Ncv32u getNumFeatures()
+ __device__ __host__ Ncv32u getNumFeatures() const
{
return (this->desc >> HaarFeatureDescriptor32_NumFeatures_Shift) & HaarFeatureDescriptor32_CreateCheck_MaxNumFeatures;
}
- __device__ __host__ Ncv32u getFeaturesOffset()
+ __device__ __host__ Ncv32u getFeaturesOffset() const
{
return this->desc & HaarFeatureDescriptor32_CreateCheck_MaxFeatureOffset;
}
return (*(Ncv32f *)&this->_ui1.x);
}
- __host__ bool isLeaf() // TODO: check this hack don't know if is correct
+ __host__ bool isLeaf() const // TODO: check this hack don't know if is correct
{
return ( _ui1.x != 0);
}
return *(Ncv32f*)&this->_ui2.x;
}
- __host__ __device__ Ncv32u getStartClassifierRootNodeOffset()
+ __host__ __device__ Ncv32u getStartClassifierRootNodeOffset() const
{
return (this->_ui2.y >> HaarStage64_Interpret_ShiftRootNodeOffset);
}
- __host__ __device__ Ncv32u getNumClassifierRootNodes()
+ __host__ __device__ Ncv32u getNumClassifierRootNodes() const
{
return (this->_ui2.y & HaarStage64_Interpret_MaskRootNodes);
}
cudaStream_t nppStSetActiveCUDAstream(cudaStream_t cudaStream);
-/*@}*/
+/**@}*/
/** \defgroup nppi NPPST Image Processing
/** Size of a buffer required for interpolation.
- *
+ *
* Requires several such buffers. See \see NppStInterpolationState.
*
* \param srcSize [IN] Frame size (both frames must be of the same size)
* \return NCV status code
*/
NCV_EXPORTS
-NCVStatus nppiStFilterRowBorder_32f_C1R(const Ncv32f *pSrc,
- NcvSize32u srcSize,
+NCVStatus nppiStFilterRowBorder_32f_C1R(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
Ncv32u nSrcStep,
- Ncv32f *pDst,
- NcvSize32u dstSize,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
Ncv32u nDstStep,
- NcvRect32u oROI,
+ NcvRect32u oROI,
NppStBorderType borderType,
- const Ncv32f *pKernel,
+ const Ncv32f *pKernel,
Ncv32s nKernelSize,
- Ncv32s nAnchor,
+ Ncv32s nAnchor,
Ncv32f multiplier);
/** Size of buffer required for vector image warping.
- *
+ *
* \param srcSize [IN] Source image size
* \param nSrcStep [IN] Source image line step
* \param hpSize [OUT] Where to store computed size (host memory)
*
* \return NCV status code
*/
-NCV_EXPORTS
+NCV_EXPORTS
NCVStatus nppiStVectorWarpGetBufferSize(NcvSize32u srcSize,
Ncv32u nSrcStep,
Ncv32u *hpSize);
* \param xFactor [IN] Row scale factor
* \param yFactor [IN] Column scale factor
* \param interpolation [IN] Interpolation type
- *
+ *
* \return NCV status code
*/
NCV_EXPORTS
Ncv64u *h_dst, Ncv32u dstStep, NcvSize32u roiSize);
-/*@}*/
+/**@}*/
/** \defgroup npps NPPST Signal Processing
Ncv32f *h_dst, Ncv32u *dstLen, Ncv32f elemRemove);
-/*@}*/
+/**@}*/
#endif // _npp_staging_hpp_
// omp_set_num_threads(1);
// Process all points in the indices vector
- //#pragma omp parallel for
for (int k = 0; k < static_cast<int> (indices_in.indices.size ()); ++k)
{
int i = indices_in.indices[k];
{
read_xml(filename,pt);
}
- catch(boost::exception const& exb)
+ catch(boost::exception const&)
{
PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to read filename with boost exception\n");
return NCV_HAAR_XML_LOADING_EXCEPTION;
level1++;
}
}
- catch(boost::exception const& exb)
+ catch(boost::exception const&)
{
PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to process content with boost exception\n");
return (NCV_HAAR_XML_LOADING_EXCEPTION);
// Process all points in the indices vector
int total = static_cast<int> (indices.size ());
-#ifdef _OPENMP
-#pragma omp parallel for
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(cloud, intr, hue, indices, mask, squared_radius, storage, total)
for (int k = 0; k < total; ++k)
{
int i = indices[k];
{
Tex2Dfetcher( const std::uint16_t* dmap, int W, int H ) : m_dmap(dmap), m_W(W), m_H(H) {}
- inline std::uint16_t operator () ( float uf, float vf )
+ inline std::uint16_t operator () ( float uf, float vf ) const
{
int u = static_cast<int>(uf);
int v = static_cast<int>(vf);
}
void
- writeXMLFile(std::string& filename)
+ writeXMLFile(std::string& filename) const
{
filebuf fb;
fb.open (filename.c_str(), ios::out);
}
void
- readXMLFile(std::string& filename)
+ readXMLFile(std::string& filename) const
{
filebuf fb;
fb.open (filename.c_str(), ios::in);
interface->stop ();
}
- pcl::visualization::CloudViewer viewer;
- pcl::people::trees::MultiTreeLiveProc* m_proc;
- cv::Mat m_lmap;
- cv::Mat m_cmap;
- cv::Mat cmap;
- cv::Mat m_bmap;
+ void load_tree(std::string treeFilenames[4], int numTrees)
+ {
+ std::ifstream fin0 (treeFilenames[0]);
+ assert(fin0.is_open());
+ m_proc = std::make_unique<pcl::people::trees::MultiTreeLiveProc> (fin0);
+
+ /// Load the other tree files
+ for (const auto& file : treeFilenames)
+ {
+ std::ifstream fin (file);
+ assert (fin.is_open());
+ m_proc->addTree(fin);
+ }
+ }
+
+ pcl::visualization::CloudViewer viewer;
+ std::unique_ptr<pcl::people::trees::MultiTreeLiveProc> m_proc;
+ cv::Mat m_lmap;
+ cv::Mat m_cmap;
+ cv::Mat cmap;
+ cv::Mat m_bmap;
};
int print_help()
PeopleTrackingApp app;
/// Load the first tree
- std::ifstream fin0(treeFilenames[0].c_str() );
- assert(fin0.is_open() );
- app.m_proc = new pcl::people::trees::MultiTreeLiveProc(fin0);
- fin0.close();
-
- /// Load the other tree files
- for(int ti=1;ti<numTrees;++ti) {
- std::ifstream fin(treeFilenames[ti].c_str() );
- assert(fin.is_open() );
- app.m_proc->addTree(fin);
- fin.close();
- }
+ app.load_tree(treeFilenames, numTrees);
+
/// Run the app
app.run();
return 0;
}
bool
-pcl::device::FacetStream::canSplit()
+pcl::device::FacetStream::canSplit() const
{
return facet_count * 3 < verts_inds.cols();
}
void compactFacets();
- bool canSplit();
+ bool canSplit() const;
void splitFacets();
private:
cudaEventDestroy(stop_);
}
- void start() { cudaEventRecord(start_, 0); }
+ void start() const { cudaEventRecord(start_, 0); }
Timer& stop() { cudaEventRecord(stop_, 0); cudaEventSynchronize(stop_); return *this; }
- float time()
+ float time() const
{
float elapsed_time;
cudaEventElapsedTime(&elapsed_time, start_, stop_);
${DSSDK_GRABBER_INCLUDES}
${RSSDK_GRABBER_INCLUDES}
${RSSDK2_GRABBER_INCLUDES}
- "include/pcl/${SUBSYS_NAME}/pxc_grabber.h" # contains only depreciation note
)
set(compression_incs
namespace pcl
{
- namespace octree
+
+namespace octree
+{
+
+using namespace std;
+
+/** \brief @b ColorCoding class
+ * \note This class encodes 8-bit color information for octree-based point cloud compression.
+ * \note
+ * \note typename: PointT: type of point used in pointcloud
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template<typename PointT>
+class ColorCoding
+{
+ // public typedefs
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudPtr = typename PointCloud::Ptr;
+ using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+public:
+ /** \brief Constructor.
+ *
+ * */
+ ColorCoding () :
+ output_ (), colorBitReduction_ (0)
+ {
+ }
+
+ /** \brief Empty class constructor. */
+ virtual
+ ~ColorCoding ()
+ {
+ }
+
+ /** \brief Define color bit depth of encoded color information.
+ * \param bitDepth_arg: amounts of bits for representing one color component
+ */
+ inline
+ void
+ setBitDepth (unsigned char bitDepth_arg)
+ {
+ colorBitReduction_ = static_cast<unsigned char> (8 - bitDepth_arg);
+ }
+
+ /** \brief Retrieve color bit depth of encoded color information.
+ * \return amounts of bits for representing one color component
+ */
+ inline unsigned char
+ getBitDepth ()
+ {
+ return (static_cast<unsigned char> (8 - colorBitReduction_));
+ }
+
+ /** \brief Set amount of voxels containing point color information and reserve memory
+ * \param voxelCount_arg: amounts of voxels
+ */
+ inline void
+ setVoxelCount (unsigned int voxelCount_arg)
+ {
+ pointAvgColorDataVector_.reserve (voxelCount_arg * 3);
+ }
+
+ /** \brief Set amount of points within point cloud to be encoded and reserve memory
+ * \param pointCount_arg: amounts of points within point cloud
+ * */
+ inline
+ void
+ setPointCount (unsigned int pointCount_arg)
+ {
+ pointDiffColorDataVector_.reserve (pointCount_arg * 3);
+ }
+
+ /** \brief Initialize encoding of color information
+ * */
+ void
+ initializeEncoding ()
+ {
+ pointAvgColorDataVector_.clear ();
+
+ pointDiffColorDataVector_.clear ();
+ }
+
+ /** \brief Initialize decoding of color information
+ * */
+ void
+ initializeDecoding ()
+ {
+ pointAvgColorDataVector_Iterator_ = pointAvgColorDataVector_.begin ();
+
+ pointDiffColorDataVector_Iterator_ = pointDiffColorDataVector_.begin ();
+ }
+
+ /** \brief Get reference to vector containing averaged color data
+ * */
+ std::vector<char>&
+ getAverageDataVector ()
+ {
+ return pointAvgColorDataVector_;
+ }
+
+ /** \brief Get reference to vector containing differential color data
+ * */
+ std::vector<char>&
+ getDifferentialDataVector ()
{
- using namespace std;
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b ColorCoding class
- * \note This class encodes 8-bit color information for octree-based point cloud compression.
- * \note
- * \note typename: PointT: type of point used in pointcloud
- * \author Julius Kammerl (julius@kammerl.de)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename PointT>
- class ColorCoding
+ return pointDiffColorDataVector_;
+ }
+
+ /** \brief Encode averaged color information for a subset of points from point cloud
+ * \param indexVector_arg indices defining a subset of points from points cloud
+ * \param rgba_offset_arg offset to color information
+ * \param inputCloud_arg input point cloud
+ * */
+ void
+ encodeAverageOfPoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
+ {
+ unsigned int avgRed = 0;
+ unsigned int avgGreen = 0;
+ unsigned int avgBlue = 0;
+
+ // iterate over points
+ std::size_t len = indexVector_arg.size ();
+ for (std::size_t i = 0; i < len; i++)
+ {
+ // get color information from points
+ const int& idx = indexVector_arg[i];
+ const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]);
+ const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
+
+ // add color information
+ avgRed += (colorInt >> 0) & 0xFF;
+ avgGreen += (colorInt >> 8) & 0xFF;
+ avgBlue += (colorInt >> 16) & 0xFF;
+
+ }
+
+ // calculated average color information
+ if (len > 1)
+ {
+ avgRed /= static_cast<unsigned int> (len);
+ avgGreen /= static_cast<unsigned int> (len);
+ avgBlue /= static_cast<unsigned int> (len);
+ }
+
+ // remove least significant bits
+ avgRed >>= colorBitReduction_;
+ avgGreen >>= colorBitReduction_;
+ avgBlue >>= colorBitReduction_;
+
+ // add to average color vector
+ pointAvgColorDataVector_.push_back (static_cast<char> (avgRed));
+ pointAvgColorDataVector_.push_back (static_cast<char> (avgGreen));
+ pointAvgColorDataVector_.push_back (static_cast<char> (avgBlue));
+ }
+
+ /** \brief Encode color information of a subset of points from point cloud
+ * \param indexVector_arg indices defining a subset of points from points cloud
+ * \param rgba_offset_arg offset to color information
+ * \param inputCloud_arg input point cloud
+ * */
+ void
+ encodePoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
+ {
+ unsigned int avgRed;
+ unsigned int avgGreen;
+ unsigned int avgBlue;
+
+ // initialize
+ avgRed = avgGreen = avgBlue = 0;
+
+ // iterate over points
+ std::size_t len = indexVector_arg.size ();
+ for (std::size_t i = 0; i < len; i++)
+ {
+ // get color information from point
+ const int& idx = indexVector_arg[i];
+ const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]);
+ const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
+
+ // add color information
+ avgRed += (colorInt >> 0) & 0xFF;
+ avgGreen += (colorInt >> 8) & 0xFF;
+ avgBlue += (colorInt >> 16) & 0xFF;
+
+ }
+
+ if (len > 1)
+ {
+ unsigned char diffRed;
+ unsigned char diffGreen;
+ unsigned char diffBlue;
+
+ // calculated average color information
+ avgRed /= static_cast<unsigned int> (len);
+ avgGreen /= static_cast<unsigned int> (len);
+ avgBlue /= static_cast<unsigned int> (len);
+
+ // iterate over points for differential encoding
+ for (std::size_t i = 0; i < len; i++)
{
+ const int& idx = indexVector_arg[i];
+ const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]);
+ const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
+
+ // extract color components and do XOR encoding with predicted average color
+ diffRed = (static_cast<unsigned char> (avgRed)) ^ static_cast<unsigned char> (((colorInt >> 0) & 0xFF));
+ diffGreen = (static_cast<unsigned char> (avgGreen)) ^ static_cast<unsigned char> (((colorInt >> 8) & 0xFF));
+ diffBlue = (static_cast<unsigned char> (avgBlue)) ^ static_cast<unsigned char> (((colorInt >> 16) & 0xFF));
+
+ // remove least significant bits
+ diffRed = static_cast<unsigned char> (diffRed >> colorBitReduction_);
+ diffGreen = static_cast<unsigned char> (diffGreen >> colorBitReduction_);
+ diffBlue = static_cast<unsigned char> (diffBlue >> colorBitReduction_);
+
+ // add to differential color vector
+ pointDiffColorDataVector_.push_back (static_cast<char> (diffRed));
+ pointDiffColorDataVector_.push_back (static_cast<char> (diffGreen));
+ pointDiffColorDataVector_.push_back (static_cast<char> (diffBlue));
+ }
+ }
+
+ // remove least significant bits from average color information
+ avgRed >>= colorBitReduction_;
+ avgGreen >>= colorBitReduction_;
+ avgBlue >>= colorBitReduction_;
+
+ // add to differential color vector
+ pointAvgColorDataVector_.push_back (static_cast<char> (avgRed));
+ pointAvgColorDataVector_.push_back (static_cast<char> (avgGreen));
+ pointAvgColorDataVector_.push_back (static_cast<char> (avgBlue));
- // public typedefs
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudPtr = typename PointCloud::Ptr;
- using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
- public:
-
- /** \brief Constructor.
- *
- * */
- ColorCoding () :
- output_ (), colorBitReduction_ (0)
- {
- }
-
- /** \brief Empty class constructor. */
- virtual
- ~ColorCoding ()
- {
- }
-
- /** \brief Define color bit depth of encoded color information.
- * \param bitDepth_arg: amounts of bits for representing one color component
- */
- inline
- void
- setBitDepth (unsigned char bitDepth_arg)
- {
- colorBitReduction_ = static_cast<unsigned char> (8 - bitDepth_arg);
- }
-
- /** \brief Retrieve color bit depth of encoded color information.
- * \return amounts of bits for representing one color component
- */
- inline unsigned char
- getBitDepth ()
- {
- return (static_cast<unsigned char> (8 - colorBitReduction_));
- }
-
- /** \brief Set amount of voxels containing point color information and reserve memory
- * \param voxelCount_arg: amounts of voxels
- */
- inline void
- setVoxelCount (unsigned int voxelCount_arg)
- {
- pointAvgColorDataVector_.reserve (voxelCount_arg * 3);
- }
-
- /** \brief Set amount of points within point cloud to be encoded and reserve memory
- * \param pointCount_arg: amounts of points within point cloud
- * */
- inline
- void
- setPointCount (unsigned int pointCount_arg)
- {
- pointDiffColorDataVector_.reserve (pointCount_arg * 3);
- }
-
- /** \brief Initialize encoding of color information
- * */
- void
- initializeEncoding ()
- {
- pointAvgColorDataVector_.clear ();
-
- pointDiffColorDataVector_.clear ();
- }
-
- /** \brief Initialize decoding of color information
- * */
- void
- initializeDecoding ()
- {
- pointAvgColorDataVector_Iterator_ = pointAvgColorDataVector_.begin ();
-
- pointDiffColorDataVector_Iterator_ = pointDiffColorDataVector_.begin ();
- }
-
- /** \brief Get reference to vector containing averaged color data
- * */
- std::vector<char>&
- getAverageDataVector ()
- {
- return pointAvgColorDataVector_;
- }
-
- /** \brief Get reference to vector containing differential color data
- * */
- std::vector<char>&
- getDifferentialDataVector ()
- {
- return pointDiffColorDataVector_;
- }
-
- /** \brief Encode averaged color information for a subset of points from point cloud
- * \param indexVector_arg indices defining a subset of points from points cloud
- * \param rgba_offset_arg offset to color information
- * \param inputCloud_arg input point cloud
- * */
- void
- encodeAverageOfPoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
- {
- unsigned int avgRed = 0;
- unsigned int avgGreen = 0;
- unsigned int avgBlue = 0;
-
- // iterate over points
- std::size_t len = indexVector_arg.size ();
- for (std::size_t i = 0; i < len; i++)
- {
- // get color information from points
- const int& idx = indexVector_arg[i];
- const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]);
- const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
-
- // add color information
- avgRed += (colorInt >> 0) & 0xFF;
- avgGreen += (colorInt >> 8) & 0xFF;
- avgBlue += (colorInt >> 16) & 0xFF;
-
- }
-
- // calculated average color information
- if (len > 1)
- {
- avgRed /= static_cast<unsigned int> (len);
- avgGreen /= static_cast<unsigned int> (len);
- avgBlue /= static_cast<unsigned int> (len);
- }
-
- // remove least significant bits
- avgRed >>= colorBitReduction_;
- avgGreen >>= colorBitReduction_;
- avgBlue >>= colorBitReduction_;
-
- // add to average color vector
- pointAvgColorDataVector_.push_back (static_cast<char> (avgRed));
- pointAvgColorDataVector_.push_back (static_cast<char> (avgGreen));
- pointAvgColorDataVector_.push_back (static_cast<char> (avgBlue));
- }
-
- /** \brief Encode color information of a subset of points from point cloud
- * \param indexVector_arg indices defining a subset of points from points cloud
- * \param rgba_offset_arg offset to color information
- * \param inputCloud_arg input point cloud
- * */
- void
- encodePoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
- {
- unsigned int avgRed;
- unsigned int avgGreen;
- unsigned int avgBlue;
-
- // initialize
- avgRed = avgGreen = avgBlue = 0;
-
- // iterate over points
- std::size_t len = indexVector_arg.size ();
- for (std::size_t i = 0; i < len; i++)
- {
- // get color information from point
- const int& idx = indexVector_arg[i];
- const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]);
- const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
-
- // add color information
- avgRed += (colorInt >> 0) & 0xFF;
- avgGreen += (colorInt >> 8) & 0xFF;
- avgBlue += (colorInt >> 16) & 0xFF;
-
- }
-
- if (len > 1)
- {
- unsigned char diffRed;
- unsigned char diffGreen;
- unsigned char diffBlue;
-
- // calculated average color information
- avgRed /= static_cast<unsigned int> (len);
- avgGreen /= static_cast<unsigned int> (len);
- avgBlue /= static_cast<unsigned int> (len);
-
- // iterate over points for differential encoding
- for (std::size_t i = 0; i < len; i++)
- {
- const int& idx = indexVector_arg[i];
- const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]);
- const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
-
- // extract color components and do XOR encoding with predicted average color
- diffRed = (static_cast<unsigned char> (avgRed)) ^ static_cast<unsigned char> (((colorInt >> 0) & 0xFF));
- diffGreen = (static_cast<unsigned char> (avgGreen)) ^ static_cast<unsigned char> (((colorInt >> 8) & 0xFF));
- diffBlue = (static_cast<unsigned char> (avgBlue)) ^ static_cast<unsigned char> (((colorInt >> 16) & 0xFF));
-
- // remove least significant bits
- diffRed = static_cast<unsigned char> (diffRed >> colorBitReduction_);
- diffGreen = static_cast<unsigned char> (diffGreen >> colorBitReduction_);
- diffBlue = static_cast<unsigned char> (diffBlue >> colorBitReduction_);
-
- // add to differential color vector
- pointDiffColorDataVector_.push_back (static_cast<char> (diffRed));
- pointDiffColorDataVector_.push_back (static_cast<char> (diffGreen));
- pointDiffColorDataVector_.push_back (static_cast<char> (diffBlue));
- }
- }
-
- // remove least significant bits from average color information
- avgRed >>= colorBitReduction_;
- avgGreen >>= colorBitReduction_;
- avgBlue >>= colorBitReduction_;
-
- // add to differential color vector
- pointAvgColorDataVector_.push_back (static_cast<char> (avgRed));
- pointAvgColorDataVector_.push_back (static_cast<char> (avgGreen));
- pointAvgColorDataVector_.push_back (static_cast<char> (avgBlue));
-
- }
-
- /** \brief Decode color information
- * \param outputCloud_arg output point cloud
- * \param beginIdx_arg index indicating first point to be assigned with color information
- * \param endIdx_arg index indicating last point to be assigned with color information
- * \param rgba_offset_arg offset to color information
- */
- void
- decodePoints (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg)
- {
- assert (beginIdx_arg <= endIdx_arg);
-
- // amount of points to be decoded
- unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
-
- // get averaged color information for current voxel
- unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++);
- unsigned char avgGreen = *(pointAvgColorDataVector_Iterator_++);
- unsigned char avgBlue = *(pointAvgColorDataVector_Iterator_++);
-
- // invert bit shifts during encoding
- avgRed = static_cast<unsigned char> (avgRed << colorBitReduction_);
- avgGreen = static_cast<unsigned char> (avgGreen << colorBitReduction_);
- avgBlue = static_cast<unsigned char> (avgBlue << colorBitReduction_);
-
- // iterate over points
- for (std::size_t i = 0; i < pointCount; i++)
- {
- unsigned int colorInt;
- if (pointCount > 1)
- {
- // get differential color information from input vector
- unsigned char diffRed = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
- unsigned char diffGreen = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
- unsigned char diffBlue = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
-
- // invert bit shifts during encoding
- diffRed = static_cast<unsigned char> (diffRed << colorBitReduction_);
- diffGreen = static_cast<unsigned char> (diffGreen << colorBitReduction_);
- diffBlue = static_cast<unsigned char> (diffBlue << colorBitReduction_);
-
- // decode color information
- colorInt = ((avgRed ^ diffRed) << 0) |
- ((avgGreen ^ diffGreen) << 8) |
- ((avgBlue ^ diffBlue) << 16);
- }
- else
- {
- // decode color information
- colorInt = (avgRed << 0) | (avgGreen << 8) | (avgBlue << 16);
- }
-
- char* idxPointPtr = reinterpret_cast<char*> (&outputCloud_arg->points[beginIdx_arg + i]);
- int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg);
- // assign color to point from point cloud
- pointColor=colorInt;
- }
- }
-
- /** \brief Set default color to points
- * \param outputCloud_arg output point cloud
- * \param beginIdx_arg index indicating first point to be assigned with color information
- * \param endIdx_arg index indicating last point to be assigned with color information
- * \param rgba_offset_arg offset to color information
- * */
- void
- setDefaultColor (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg)
- {
- assert (beginIdx_arg <= endIdx_arg);
-
- // amount of points to be decoded
- unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
-
- // iterate over points
- for (std::size_t i = 0; i < pointCount; i++)
- {
- char* idxPointPtr = reinterpret_cast<char*> (&outputCloud_arg->points[beginIdx_arg + i]);
- int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg);
- // assign color to point from point cloud
- pointColor = defaultColor_;
- }
- }
-
-
- protected:
-
- /** \brief Pointer to output point cloud dataset. */
- PointCloudPtr output_;
-
- /** \brief Vector for storing average color information */
- std::vector<char> pointAvgColorDataVector_;
-
- /** \brief Iterator on average color information vector */
- std::vector<char>::const_iterator pointAvgColorDataVector_Iterator_;
-
- /** \brief Vector for storing differential color information */
- std::vector<char> pointDiffColorDataVector_;
-
- /** \brief Iterator on differential color information vector */
- std::vector<char>::const_iterator pointDiffColorDataVector_Iterator_;
-
- /** \brief Amount of bits to be removed from color components before encoding */
- unsigned char colorBitReduction_;
-
- // frame header identifier
- static const int defaultColor_;
-
- };
-
- // define default color
- template<typename PointT>
- const int ColorCoding<PointT>::defaultColor_ = ((255) << 0) |
- ((255) << 8) |
- ((255) << 16);
+ }
+ /** \brief Decode color information
+ * \param outputCloud_arg output point cloud
+ * \param beginIdx_arg index indicating first point to be assigned with color information
+ * \param endIdx_arg index indicating last point to be assigned with color information
+ * \param rgba_offset_arg offset to color information
+ */
+ void
+ decodePoints (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg)
+ {
+ assert (beginIdx_arg <= endIdx_arg);
+
+ // amount of points to be decoded
+ unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
+
+ // get averaged color information for current voxel
+ unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++);
+ unsigned char avgGreen = *(pointAvgColorDataVector_Iterator_++);
+ unsigned char avgBlue = *(pointAvgColorDataVector_Iterator_++);
+
+ // invert bit shifts during encoding
+ avgRed = static_cast<unsigned char> (avgRed << colorBitReduction_);
+ avgGreen = static_cast<unsigned char> (avgGreen << colorBitReduction_);
+ avgBlue = static_cast<unsigned char> (avgBlue << colorBitReduction_);
+
+ // iterate over points
+ for (std::size_t i = 0; i < pointCount; i++)
+ {
+ unsigned int colorInt;
+ if (pointCount > 1)
+ {
+ // get differential color information from input vector
+ unsigned char diffRed = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
+ unsigned char diffGreen = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
+ unsigned char diffBlue = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
+
+ // invert bit shifts during encoding
+ diffRed = static_cast<unsigned char> (diffRed << colorBitReduction_);
+ diffGreen = static_cast<unsigned char> (diffGreen << colorBitReduction_);
+ diffBlue = static_cast<unsigned char> (diffBlue << colorBitReduction_);
+
+ // decode color information
+ colorInt = ((avgRed ^ diffRed) << 0) |
+ ((avgGreen ^ diffGreen) << 8) |
+ ((avgBlue ^ diffBlue) << 16);
+ }
+ else
+ {
+ // decode color information
+ colorInt = (avgRed << 0) | (avgGreen << 8) | (avgBlue << 16);
+ }
+
+ char* idxPointPtr = reinterpret_cast<char*> (&outputCloud_arg->points[beginIdx_arg + i]);
+ int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg);
+ // assign color to point from point cloud
+ pointColor=colorInt;
+ }
+ }
+
+ /** \brief Set default color to points
+ * \param outputCloud_arg output point cloud
+ * \param beginIdx_arg index indicating first point to be assigned with color information
+ * \param endIdx_arg index indicating last point to be assigned with color information
+ * \param rgba_offset_arg offset to color information
+ * */
+ void
+ setDefaultColor (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg)
+ {
+ assert (beginIdx_arg <= endIdx_arg);
+
+ // amount of points to be decoded
+ unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
+
+ // iterate over points
+ for (std::size_t i = 0; i < pointCount; i++)
+ {
+ char* idxPointPtr = reinterpret_cast<char*> (&outputCloud_arg->points[beginIdx_arg + i]);
+ int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg);
+ // assign color to point from point cloud
+ pointColor = defaultColor_;
+ }
}
-}
+
+protected:
+ /** \brief Pointer to output point cloud dataset. */
+ PointCloudPtr output_;
+
+ /** \brief Vector for storing average color information */
+ std::vector<char> pointAvgColorDataVector_;
+
+ /** \brief Iterator on average color information vector */
+ std::vector<char>::const_iterator pointAvgColorDataVector_Iterator_;
+
+ /** \brief Vector for storing differential color information */
+ std::vector<char> pointDiffColorDataVector_;
+
+ /** \brief Iterator on differential color information vector */
+ std::vector<char>::const_iterator pointDiffColorDataVector_Iterator_;
+
+ /** \brief Amount of bits to be removed from color components before encoding */
+ unsigned char colorBitReduction_;
+
+ // frame header identifier
+ static const int defaultColor_;
+};
+
+// define default color
+template<typename PointT>
+const int ColorCoding<PointT>::defaultColor_ = ((255) << 0) |
+ ((255) << 8) |
+ ((255) << 16);
+
+} // namespace octree
+} // namespace pcl
#define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding<T>;
+
* \param n_arg: some value
* \return binary logarithm (log2) of argument n_arg
*/
- PCL_DEPRECATED("use std::log2 instead")
+ PCL_DEPRECATED(1, 12, "use std::log2 instead")
inline double
Log2 (double n_arg)
{
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <vector>
#include <limits>
namespace pcl
{
- namespace io
- {
+namespace io
+{
- template<typename PointT> struct CompressionPointTraits
- {
- static const bool hasColor = false;
- static const unsigned int channels = 1;
- static const std::size_t bytesPerPoint = 3 * sizeof(float);
- };
+template<typename PointT> struct CompressionPointTraits
+{
+ static const bool hasColor = false;
+ static const unsigned int channels = 1;
+ static const std::size_t bytesPerPoint = 3 * sizeof(float);
+};
- template<>
-
struct CompressionPointTraits<PointXYZRGB>
- {
- static const bool hasColor = true;
- static const unsigned int channels = 4;
- static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t);
- };
+template<>
+struct CompressionPointTraits<PointXYZRGB>
+{
+ static const bool hasColor = true;
+ static const unsigned int channels = 4;
+ static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t);
+};
- template<>
- struct CompressionPointTraits<PointXYZRGBA>
- {
- static const bool hasColor = true;
- static const unsigned int channels = 4;
- static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t);
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- template <typename PointT, bool enableColor = CompressionPointTraits<PointT>::hasColor >
- struct OrganizedConversion;
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- // Uncolored point cloud specialization
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename PointT>
- struct OrganizedConversion<PointT, false>
- {
- /** \brief Convert point cloud to disparity image
- * \param[in] cloud_arg input point cloud
- * \param[in] focalLength_arg focal length
- * \param[in] disparityShift_arg disparity shift
- * \param[in] disparityScale_arg disparity scaling
- * \param[out] disparityData_arg output disparity image
- * \ingroup io
- */
- static void convert(const pcl::PointCloud<PointT>& cloud_arg,
- float focalLength_arg,
- float disparityShift_arg,
- float disparityScale_arg,
- bool ,
- typename std::vector<std::uint16_t>& disparityData_arg,
- typename std::vector<std::uint8_t>&)
- {
- std::size_t cloud_size = cloud_arg.points.size ();
+template<>
+struct CompressionPointTraits<PointXYZRGBA>
+{
+ static const bool hasColor = true;
+ static const unsigned int channels = 4;
+ static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t);
+};
- // Clear image data
- disparityData_arg.clear ();
+template <typename PointT, bool enableColor = CompressionPointTraits<PointT>::hasColor >
+struct OrganizedConversion;
- disparityData_arg.reserve (cloud_size);
+// Uncolored point cloud specialization
+template<typename PointT>
+struct OrganizedConversion<PointT, false>
+{
+ /** \brief Convert point cloud to disparity image
+ * \param[in] cloud_arg input point cloud
+ * \param[in] focalLength_arg focal length
+ * \param[in] disparityShift_arg disparity shift
+ * \param[in] disparityScale_arg disparity scaling
+ * \param[out] disparityData_arg output disparity image
+ * \ingroup io
+ */
+ static void convert(const pcl::PointCloud<PointT>& cloud_arg,
+ float focalLength_arg,
+ float disparityShift_arg,
+ float disparityScale_arg,
+ bool ,
+ typename std::vector<std::uint16_t>& disparityData_arg,
+ typename std::vector<std::uint8_t>&)
+ {
+ std::size_t cloud_size = cloud_arg.points.size ();
- for (std::size_t i = 0; i < cloud_size; ++i)
- {
- // Get point from cloud
- const PointT& point = cloud_arg.points[i];
+ // Clear image data
+ disparityData_arg.clear ();
- if (pcl::isFinite (point))
- {
- // Inverse depth quantization
- std::uint16_t disparity = static_cast<std::uint16_t> ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
- disparityData_arg.push_back (disparity);
- }
- else
- {
- // Non-valid points are encoded with zeros
- disparityData_arg.push_back (0);
- }
- }
- }
+ disparityData_arg.reserve (cloud_size);
- /** \brief Convert disparity image to point cloud
- * \param[in] disparityData_arg input depth image
- * \param[in] width_arg width of disparity image
- * \param[in] height_arg height of disparity image
- * \param[in] focalLength_arg focal length
- * \param[in] disparityShift_arg disparity shift
- * \param[in] disparityScale_arg disparity scaling
- * \param[out] cloud_arg output point cloud
- * \ingroup io
- */
- static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
- typename std::vector<std::uint8_t>&,
- bool,
- std::size_t width_arg,
- std::size_t height_arg,
- float focalLength_arg,
- float disparityShift_arg,
- float disparityScale_arg,
- pcl::PointCloud<PointT>& cloud_arg)
+ for (std::size_t i = 0; i < cloud_size; ++i)
+ {
+ // Get point from cloud
+ const PointT& point = cloud_arg.points[i];
+
+ if (pcl::isFinite (point))
{
- std::size_t cloud_size = width_arg * height_arg;
+ // Inverse depth quantization
+ std::uint16_t disparity = static_cast<std::uint16_t> ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
+ disparityData_arg.push_back (disparity);
+ }
+ else
+ {
+ // Non-valid points are encoded with zeros
+ disparityData_arg.push_back (0);
+ }
+ }
+ }
- assert(disparityData_arg.size()==cloud_size);
+ /** \brief Convert disparity image to point cloud
+ * \param[in] disparityData_arg input depth image
+ * \param[in] width_arg width of disparity image
+ * \param[in] height_arg height of disparity image
+ * \param[in] focalLength_arg focal length
+ * \param[in] disparityShift_arg disparity shift
+ * \param[in] disparityScale_arg disparity scaling
+ * \param[out] cloud_arg output point cloud
+ * \ingroup io
+ */
+ static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
+ typename std::vector<std::uint8_t>&,
+ bool,
+ std::size_t width_arg,
+ std::size_t height_arg,
+ float focalLength_arg,
+ float disparityShift_arg,
+ float disparityScale_arg,
+ pcl::PointCloud<PointT>& cloud_arg)
+ {
+ std::size_t cloud_size = width_arg * height_arg;
- // Reset point cloud
- cloud_arg.points.clear ();
- cloud_arg.points.reserve (cloud_size);
+ assert(disparityData_arg.size()==cloud_size);
- // Define point cloud parameters
- cloud_arg.width = static_cast<std::uint32_t> (width_arg);
- cloud_arg.height = static_cast<std::uint32_t> (height_arg);
- cloud_arg.is_dense = false;
+ // Reset point cloud
+ cloud_arg.points.clear ();
+ cloud_arg.points.reserve (cloud_size);
- // Calculate center of disparity image
- int centerX = static_cast<int> (width_arg / 2);
- int centerY = static_cast<int> (height_arg / 2);
+ // Define point cloud parameters
+ cloud_arg.width = static_cast<std::uint32_t> (width_arg);
+ cloud_arg.height = static_cast<std::uint32_t> (height_arg);
+ cloud_arg.is_dense = false;
- const float fl_const = 1.0f / focalLength_arg;
- static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ // Calculate center of disparity image
+ int centerX = static_cast<int> (width_arg / 2);
+ int centerY = static_cast<int> (height_arg / 2);
- std::size_t i = 0;
- for (int y = -centerY; y < centerY; ++y )
- for (int x = -centerX; x < centerX; ++x )
- {
- PointT newPoint;
- const std::uint16_t& pixel_disparity = disparityData_arg[i];
- ++i;
+ const float fl_const = 1.0f / focalLength_arg;
+ static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
- if (pixel_disparity)
- {
- // Inverse depth decoding
- float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
+ std::size_t i = 0;
+ for (int y = -centerY; y < centerY; ++y )
+ for (int x = -centerX; x < centerX; ++x )
+ {
+ PointT newPoint;
+ const std::uint16_t& pixel_disparity = disparityData_arg[i];
+ ++i;
- // Generate new points
- newPoint.x = static_cast<float> (x) * depth * fl_const;
- newPoint.y = static_cast<float> (y) * depth * fl_const;
- newPoint.z = depth;
+ if (pixel_disparity)
+ {
+ // Inverse depth decoding
+ float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
- }
- else
- {
- // Generate bad point
- newPoint.x = newPoint.y = newPoint.z = bad_point;
- }
+ // Generate new points
+ newPoint.x = static_cast<float> (x) * depth * fl_const;
+ newPoint.y = static_cast<float> (y) * depth * fl_const;
+ newPoint.z = depth;
- cloud_arg.points.push_back (newPoint);
- }
+ }
+ else
+ {
+ // Generate bad point
+ newPoint.x = newPoint.y = newPoint.z = bad_point;
+ }
+ cloud_arg.points.push_back (newPoint);
}
+ }
+ /** \brief Convert disparity image to point cloud
+ * \param[in] depthData_arg input depth image
+ * \param[in] width_arg width of disparity image
+ * \param[in] height_arg height of disparity image
+ * \param[in] focalLength_arg focal length
+ * \param[out] cloud_arg output point cloud
+ * \ingroup io
+ */
+ static void convert(typename std::vector<float>& depthData_arg,
+ typename std::vector<std::uint8_t>&,
+ bool,
+ std::size_t width_arg,
+ std::size_t height_arg,
+ float focalLength_arg,
+ pcl::PointCloud<PointT>& cloud_arg)
+ {
+ std::size_t cloud_size = width_arg * height_arg;
- /** \brief Convert disparity image to point cloud
- * \param[in] depthData_arg input depth image
- * \param[in] width_arg width of disparity image
- * \param[in] height_arg height of disparity image
- * \param[in] focalLength_arg focal length
- * \param[out] cloud_arg output point cloud
- * \ingroup io
- */
- static void convert(typename std::vector<float>& depthData_arg,
- typename std::vector<std::uint8_t>&,
- bool,
- std::size_t width_arg,
- std::size_t height_arg,
- float focalLength_arg,
- pcl::PointCloud<PointT>& cloud_arg)
- {
- std::size_t cloud_size = width_arg * height_arg;
-
- assert(depthData_arg.size()==cloud_size);
-
- // Reset point cloud
- cloud_arg.points.clear ();
- cloud_arg.points.reserve (cloud_size);
+ assert(depthData_arg.size()==cloud_size);
- // Define point cloud parameters
- cloud_arg.width = static_cast<std::uint32_t> (width_arg);
- cloud_arg.height = static_cast<std::uint32_t> (height_arg);
- cloud_arg.is_dense = false;
+ // Reset point cloud
+ cloud_arg.points.clear ();
+ cloud_arg.points.reserve (cloud_size);
- // Calculate center of disparity image
- int centerX = static_cast<int> (width_arg / 2);
- int centerY = static_cast<int> (height_arg / 2);
+ // Define point cloud parameters
+ cloud_arg.width = static_cast<std::uint32_t> (width_arg);
+ cloud_arg.height = static_cast<std::uint32_t> (height_arg);
+ cloud_arg.is_dense = false;
- const float fl_const = 1.0f / focalLength_arg;
- static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ // Calculate center of disparity image
+ int centerX = static_cast<int> (width_arg / 2);
+ int centerY = static_cast<int> (height_arg / 2);
- std::size_t i = 0;
- for (int y = -centerY; y < centerY; ++y )
- for (int x = -centerX; x < centerX; ++x )
- {
- PointT newPoint;
- const float& pixel_depth = depthData_arg[i];
- ++i;
+ const float fl_const = 1.0f / focalLength_arg;
+ static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
- if (pixel_depth)
- {
- // Inverse depth decoding
- float depth = focalLength_arg / pixel_depth;
+ std::size_t i = 0;
+ for (int y = -centerY; y < centerY; ++y )
+ for (int x = -centerX; x < centerX; ++x )
+ {
+ PointT newPoint;
+ const float& pixel_depth = depthData_arg[i];
+ ++i;
- // Generate new points
- newPoint.x = static_cast<float> (x) * depth * fl_const;
- newPoint.y = static_cast<float> (y) * depth * fl_const;
- newPoint.z = depth;
+ if (pixel_depth)
+ {
+ // Inverse depth decoding
+ float depth = focalLength_arg / pixel_depth;
- }
- else
- {
- // Generate bad point
- newPoint.x = newPoint.y = newPoint.z = bad_point;
- }
+ // Generate new points
+ newPoint.x = static_cast<float> (x) * depth * fl_const;
+ newPoint.y = static_cast<float> (y) * depth * fl_const;
+ newPoint.z = depth;
- cloud_arg.points.push_back (newPoint);
- }
+ }
+ else
+ {
+ // Generate bad point
+ newPoint.x = newPoint.y = newPoint.z = bad_point;
+ }
+ cloud_arg.points.push_back (newPoint);
}
+ }
+};
+
+// Colored point cloud specialization
+template <typename PointT>
+struct OrganizedConversion<PointT, true>
+{
+ /** \brief Convert point cloud to disparity image and rgb image
+ * \param[in] cloud_arg input point cloud
+ * \param[in] focalLength_arg focal length
+ * \param[in] disparityShift_arg disparity shift
+ * \param[in] disparityScale_arg disparity scaling
+ * \param[in] convertToMono convert color to mono/grayscale
+ * \param[out] disparityData_arg output disparity image
+ * \param[out] rgbData_arg output rgb image
+ * \ingroup io
+ */
+ static void convert(const pcl::PointCloud<PointT>& cloud_arg,
+ float focalLength_arg,
+ float disparityShift_arg,
+ float disparityScale_arg,
+ bool convertToMono,
+ typename std::vector<std::uint16_t>& disparityData_arg,
+ typename std::vector<std::uint8_t>& rgbData_arg)
+ {
+ std::size_t cloud_size = cloud_arg.points.size ();
- };
+ // Reset output vectors
+ disparityData_arg.clear ();
+ rgbData_arg.clear ();
- //////////////////////////////////////////////////////////////////////////////////////////////
- // Colored point cloud specialization
- //////////////////////////////////////////////////////////////////////////////////////////////
- template <typename PointT>
- struct OrganizedConversion<PointT, true>
+ // Allocate memory
+ disparityData_arg.reserve (cloud_size);
+ if (convertToMono)
{
- /** \brief Convert point cloud to disparity image and rgb image
- * \param[in] cloud_arg input point cloud
- * \param[in] focalLength_arg focal length
- * \param[in] disparityShift_arg disparity shift
- * \param[in] disparityScale_arg disparity scaling
- * \param[in] convertToMono convert color to mono/grayscale
- * \param[out] disparityData_arg output disparity image
- * \param[out] rgbData_arg output rgb image
- * \ingroup io
- */
- static void convert(const pcl::PointCloud<PointT>& cloud_arg,
- float focalLength_arg,
- float disparityShift_arg,
- float disparityScale_arg,
- bool convertToMono,
- typename std::vector<std::uint16_t>& disparityData_arg,
- typename std::vector<std::uint8_t>& rgbData_arg)
- {
- std::size_t cloud_size = cloud_arg.points.size ();
+ rgbData_arg.reserve (cloud_size);
+ } else
+ {
+ rgbData_arg.reserve (cloud_size * 3);
+ }
- // Reset output vectors
- disparityData_arg.clear ();
- rgbData_arg.clear ();
+ for (std::size_t i = 0; i < cloud_size; ++i)
+ {
+ const PointT& point = cloud_arg.points[i];
- // Allocate memory
- disparityData_arg.reserve (cloud_size);
+ if (pcl::isFinite (point))
+ {
if (convertToMono)
{
- rgbData_arg.reserve (cloud_size);
+ // Encode point color
+ std::uint8_t grayvalue = static_cast<std::uint8_t>(0.2989 * point.r
+ + 0.5870 * point.g
+ + 0.1140 * point.b);
+
+ rgbData_arg.push_back (grayvalue);
} else
{
- rgbData_arg.reserve (cloud_size * 3);
+ // Encode point color
+ rgbData_arg.push_back (point.r);
+ rgbData_arg.push_back (point.g);
+ rgbData_arg.push_back (point.b);
}
+ // Inverse depth quantization
+ std::uint16_t disparity = static_cast<std::uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
- for (std::size_t i = 0; i < cloud_size; ++i)
+ // Encode disparity
+ disparityData_arg.push_back (disparity);
+ }
+ else
+ {
+ // Encode black point
+ if (convertToMono)
{
- const PointT& point = cloud_arg.points[i];
-
- if (pcl::isFinite (point))
- {
- if (convertToMono)
- {
- // Encode point color
- std::uint8_t grayvalue = static_cast<std::uint8_t>(0.2989 * point.r
- + 0.5870 * point.g
- + 0.1140 * point.b);
+ rgbData_arg.push_back (0);
+ } else
+ {
+ rgbData_arg.push_back (0);
+ rgbData_arg.push_back (0);
+ rgbData_arg.push_back (0);
+ }
- rgbData_arg.push_back (grayvalue);
- } else
- {
- // Encode point color
- rgbData_arg.push_back (point.r);
- rgbData_arg.push_back (point.g);
- rgbData_arg.push_back (point.b);
- }
+ // Encode bad point
+ disparityData_arg.push_back (0);
+ }
+ }
+ }
+ /** \brief Convert disparity image to point cloud
+ * \param[in] disparityData_arg output disparity image
+ * \param[in] rgbData_arg output rgb image
+ * \param[in] monoImage_arg input image is a single-channel mono image
+ * \param[in] width_arg width of disparity image
+ * \param[in] height_arg height of disparity image
+ * \param[in] focalLength_arg focal length
+ * \param[in] disparityShift_arg disparity shift
+ * \param[in] disparityScale_arg disparity scaling
+ * \param[out] cloud_arg output point cloud
+ * \ingroup io
+ */
+ static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
+ typename std::vector<std::uint8_t>& rgbData_arg,
+ bool monoImage_arg,
+ std::size_t width_arg,
+ std::size_t height_arg,
+ float focalLength_arg,
+ float disparityShift_arg,
+ float disparityScale_arg,
+ pcl::PointCloud<PointT>& cloud_arg)
+ {
+ std::size_t cloud_size = width_arg*height_arg;
+ bool hasColor = (!rgbData_arg.empty ());
- // Inverse depth quantization
- std::uint16_t disparity = static_cast<std::uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
+ // Check size of input data
+ assert (disparityData_arg.size()==cloud_size);
+ if (hasColor)
+ {
+ if (monoImage_arg)
+ {
+ assert (rgbData_arg.size()==cloud_size);
+ } else
+ {
+ assert (rgbData_arg.size()==cloud_size*3);
+ }
+ }
- // Encode disparity
- disparityData_arg.push_back (disparity);
- }
- else
- {
+ // Reset point cloud
+ cloud_arg.points.clear();
+ cloud_arg.points.reserve(cloud_size);
- // Encode black point
- if (convertToMono)
- {
- rgbData_arg.push_back (0);
- } else
- {
- rgbData_arg.push_back (0);
- rgbData_arg.push_back (0);
- rgbData_arg.push_back (0);
- }
+ // Define point cloud parameters
+ cloud_arg.width = static_cast<std::uint32_t>(width_arg);
+ cloud_arg.height = static_cast<std::uint32_t>(height_arg);
+ cloud_arg.is_dense = false;
- // Encode bad point
- disparityData_arg.push_back (0);
- }
- }
+ // Calculate center of disparity image
+ int centerX = static_cast<int>(width_arg/2);
+ int centerY = static_cast<int>(height_arg/2);
- }
+ const float fl_const = 1.0f/focalLength_arg;
+ static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
- /** \brief Convert disparity image to point cloud
- * \param[in] disparityData_arg output disparity image
- * \param[in] rgbData_arg output rgb image
- * \param[in] monoImage_arg input image is a single-channel mono image
- * \param[in] width_arg width of disparity image
- * \param[in] height_arg height of disparity image
- * \param[in] focalLength_arg focal length
- * \param[in] disparityShift_arg disparity shift
- * \param[in] disparityScale_arg disparity scaling
- * \param[out] cloud_arg output point cloud
- * \ingroup io
- */
- static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
- typename std::vector<std::uint8_t>& rgbData_arg,
- bool monoImage_arg,
- std::size_t width_arg,
- std::size_t height_arg,
- float focalLength_arg,
- float disparityShift_arg,
- float disparityScale_arg,
- pcl::PointCloud<PointT>& cloud_arg)
+ std::size_t i = 0;
+ for (int y = -centerY; y < centerY; ++y )
+ for (int x = -centerX; x < centerX; ++x )
{
- std::size_t cloud_size = width_arg*height_arg;
- bool hasColor = (!rgbData_arg.empty ());
-
- // Check size of input data
- assert (disparityData_arg.size()==cloud_size);
- if (hasColor)
- {
- if (monoImage_arg)
- {
- assert (rgbData_arg.size()==cloud_size);
- } else
- {
- assert (rgbData_arg.size()==cloud_size*3);
- }
- }
-
- // Reset point cloud
- cloud_arg.points.clear();
- cloud_arg.points.reserve(cloud_size);
+ PointT newPoint;
- // Define point cloud parameters
- cloud_arg.width = static_cast<std::uint32_t>(width_arg);
- cloud_arg.height = static_cast<std::uint32_t>(height_arg);
- cloud_arg.is_dense = false;
+ const std::uint16_t& pixel_disparity = disparityData_arg[i];
- // Calculate center of disparity image
- int centerX = static_cast<int>(width_arg/2);
- int centerY = static_cast<int>(height_arg/2);
+ if (pixel_disparity && (pixel_disparity!=0x7FF))
+ {
+ float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
- const float fl_const = 1.0f/focalLength_arg;
- static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ // Define point location
+ newPoint.z = depth;
+ newPoint.x = static_cast<float> (x) * depth * fl_const;
+ newPoint.y = static_cast<float> (y) * depth * fl_const;
- std::size_t i = 0;
- for (int y = -centerY; y < centerY; ++y )
- for (int x = -centerX; x < centerX; ++x )
+ if (hasColor)
{
- PointT newPoint;
-
- const std::uint16_t& pixel_disparity = disparityData_arg[i];
-
- if (pixel_disparity && (pixel_disparity!=0x7FF))
+ if (monoImage_arg)
{
- float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
-
- // Define point location
- newPoint.z = depth;
- newPoint.x = static_cast<float> (x) * depth * fl_const;
- newPoint.y = static_cast<float> (y) * depth * fl_const;
-
- if (hasColor)
- {
- if (monoImage_arg)
- {
- // Define point color
- newPoint.r = rgbData_arg[i];
- newPoint.g = rgbData_arg[i];
- newPoint.b = rgbData_arg[i];
- } else
- {
- // Define point color
- newPoint.r = rgbData_arg[i*3+0];
- newPoint.g = rgbData_arg[i*3+1];
- newPoint.b = rgbData_arg[i*3+2];
- }
-
- } else
- {
- // Set white point color
- newPoint.rgba = 0xffffffffu;
- }
+ // Define point color
+ newPoint.r = rgbData_arg[i];
+ newPoint.g = rgbData_arg[i];
+ newPoint.b = rgbData_arg[i];
} else
{
- // Define bad point
- newPoint.x = newPoint.y = newPoint.z = bad_point;
- newPoint.rgb = 0.0f;
+ // Define point color
+ newPoint.r = rgbData_arg[i*3+0];
+ newPoint.g = rgbData_arg[i*3+1];
+ newPoint.b = rgbData_arg[i*3+2];
}
- // Add point to cloud
- cloud_arg.points.push_back(newPoint);
- // Increment point iterator
- ++i;
- }
- }
-
- /** \brief Convert disparity image to point cloud
- * \param[in] depthData_arg output disparity image
- * \param[in] rgbData_arg output rgb image
- * \param[in] monoImage_arg input image is a single-channel mono image
- * \param[in] width_arg width of disparity image
- * \param[in] height_arg height of disparity image
- * \param[in] focalLength_arg focal length
- * \param[out] cloud_arg output point cloud
- * \ingroup io
- */
- static void convert(typename std::vector<float>& depthData_arg,
- typename std::vector<std::uint8_t>& rgbData_arg,
- bool monoImage_arg,
- std::size_t width_arg,
- std::size_t height_arg,
- float focalLength_arg,
- pcl::PointCloud<PointT>& cloud_arg)
- {
- std::size_t cloud_size = width_arg*height_arg;
- bool hasColor = (!rgbData_arg.empty ());
-
- // Check size of input data
- assert (depthData_arg.size()==cloud_size);
- if (hasColor)
- {
- if (monoImage_arg)
- {
- assert (rgbData_arg.size()==cloud_size);
} else
{
- assert (rgbData_arg.size()==cloud_size*3);
+ // Set white point color
+ newPoint.rgba = 0xffffffffu;
}
+ } else
+ {
+ // Define bad point
+ newPoint.x = newPoint.y = newPoint.z = bad_point;
+ newPoint.rgb = 0.0f;
}
- // Reset point cloud
- cloud_arg.points.clear();
- cloud_arg.points.reserve(cloud_size);
+ // Add point to cloud
+ cloud_arg.points.push_back(newPoint);
+ // Increment point iterator
+ ++i;
+ }
+ }
- // Define point cloud parameters
- cloud_arg.width = static_cast<std::uint32_t>(width_arg);
- cloud_arg.height = static_cast<std::uint32_t>(height_arg);
- cloud_arg.is_dense = false;
+ /** \brief Convert disparity image to point cloud
+ * \param[in] depthData_arg output disparity image
+ * \param[in] rgbData_arg output rgb image
+ * \param[in] monoImage_arg input image is a single-channel mono image
+ * \param[in] width_arg width of disparity image
+ * \param[in] height_arg height of disparity image
+ * \param[in] focalLength_arg focal length
+ * \param[out] cloud_arg output point cloud
+ * \ingroup io
+ */
+ static void convert(typename std::vector<float>& depthData_arg,
+ typename std::vector<std::uint8_t>& rgbData_arg,
+ bool monoImage_arg,
+ std::size_t width_arg,
+ std::size_t height_arg,
+ float focalLength_arg,
+ pcl::PointCloud<PointT>& cloud_arg)
+ {
+ std::size_t cloud_size = width_arg*height_arg;
+ bool hasColor = (!rgbData_arg.empty ());
- // Calculate center of disparity image
- int centerX = static_cast<int>(width_arg/2);
- int centerY = static_cast<int>(height_arg/2);
+ // Check size of input data
+ assert (depthData_arg.size()==cloud_size);
+ if (hasColor)
+ {
+ if (monoImage_arg)
+ {
+ assert (rgbData_arg.size()==cloud_size);
+ } else
+ {
+ assert (rgbData_arg.size()==cloud_size*3);
+ }
+ }
- const float fl_const = 1.0f/focalLength_arg;
- static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ // Reset point cloud
+ cloud_arg.points.clear();
+ cloud_arg.points.reserve(cloud_size);
- std::size_t i = 0;
- for (int y = -centerY; y < centerY; ++y )
- for (int x = -centerX; x < centerX; ++x )
- {
- PointT newPoint;
+ // Define point cloud parameters
+ cloud_arg.width = static_cast<std::uint32_t>(width_arg);
+ cloud_arg.height = static_cast<std::uint32_t>(height_arg);
+ cloud_arg.is_dense = false;
- const float& pixel_depth = depthData_arg[i];
+ // Calculate center of disparity image
+ int centerX = static_cast<int>(width_arg/2);
+ int centerY = static_cast<int>(height_arg/2);
- if (pixel_depth==pixel_depth)
+ const float fl_const = 1.0f/focalLength_arg;
+ static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+ std::size_t i = 0;
+ for (int y = -centerY; y < centerY; ++y )
+ for (int x = -centerX; x < centerX; ++x )
+ {
+ PointT newPoint;
+
+ const float& pixel_depth = depthData_arg[i];
+
+ if (pixel_depth==pixel_depth)
+ {
+ float depth = focalLength_arg / pixel_depth;
+
+ // Define point location
+ newPoint.z = depth;
+ newPoint.x = static_cast<float> (x) * depth * fl_const;
+ newPoint.y = static_cast<float> (y) * depth * fl_const;
+
+ if (hasColor)
+ {
+ if (monoImage_arg)
{
- float depth = focalLength_arg / pixel_depth;
-
- // Define point location
- newPoint.z = depth;
- newPoint.x = static_cast<float> (x) * depth * fl_const;
- newPoint.y = static_cast<float> (y) * depth * fl_const;
-
- if (hasColor)
- {
- if (monoImage_arg)
- {
- // Define point color
- newPoint.r = rgbData_arg[i];
- newPoint.g = rgbData_arg[i];
- newPoint.b = rgbData_arg[i];
- } else
- {
- // Define point color
- newPoint.r = rgbData_arg[i*3+0];
- newPoint.g = rgbData_arg[i*3+1];
- newPoint.b = rgbData_arg[i*3+2];
- }
-
- } else
- {
- // Set white point color
- newPoint.rgba = 0xffffffffu;
- }
+ // Define point color
+ newPoint.r = rgbData_arg[i];
+ newPoint.g = rgbData_arg[i];
+ newPoint.b = rgbData_arg[i];
} else
{
- // Define bad point
- newPoint.x = newPoint.y = newPoint.z = bad_point;
- newPoint.rgb = 0.0f;
+ // Define point color
+ newPoint.r = rgbData_arg[i*3+0];
+ newPoint.g = rgbData_arg[i*3+1];
+ newPoint.b = rgbData_arg[i*3+2];
}
- // Add point to cloud
- cloud_arg.points.push_back(newPoint);
- // Increment point iterator
- ++i;
+ } else
+ {
+ // Set white point color
+ newPoint.rgba = 0xffffffffu;
+ }
+ } else
+ {
+ // Define bad point
+ newPoint.x = newPoint.y = newPoint.z = bad_point;
+ newPoint.rgb = 0.0f;
}
- }
- };
+ // Add point to cloud
+ cloud_arg.points.push_back(newPoint);
+ // Increment point iterator
+ ++i;
+ }
}
-}
+};
+
+} // namespace io
+} // namespace pcl
+
namespace pcl
{
- namespace octree
- {
- /** \brief @b PointCoding class
- * \note This class encodes 8-bit differential point information for octree-based point cloud compression.
- * \note typename: PointT: type of point used in pointcloud
- * \author Julius Kammerl (julius@kammerl.de)
+namespace octree
+{
+/** \brief @b PointCoding class
+ * \note This class encodes 8-bit differential point information for octree-based point cloud compression.
+ * \note typename: PointT: type of point used in pointcloud
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template<typename PointT>
+class PointCoding
+{
+ // public typedefs
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudPtr = typename PointCloud::Ptr;
+ using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+ public:
+ /** \brief Constructor. */
+ PointCoding () :
+ output_ (),
+ pointCompressionResolution_ (0.001f) // 1mm
+ {
+ }
+
+ /** \brief Empty class constructor. */
+ virtual
+ ~PointCoding ()
+ {
+ }
+
+ /** \brief Define precision of point information
+ * \param precision_arg: precision
+ */
+ inline void
+ setPrecision (float precision_arg)
+ {
+ pointCompressionResolution_ = precision_arg;
+ }
+
+ /** \brief Retrieve precision of point information
+ * \return precision
*/
- template<typename PointT>
- class PointCoding
+ inline float
+ getPrecision ()
{
- // public typedefs
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudPtr = typename PointCloud::Ptr;
- using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
- public:
- /** \brief Constructor. */
- PointCoding () :
- output_ (),
- pointCompressionResolution_ (0.001f) // 1mm
- {
- }
-
- /** \brief Empty class constructor. */
- virtual
- ~PointCoding ()
- {
- }
-
- /** \brief Define precision of point information
- * \param precision_arg: precision
- */
- inline void
- setPrecision (float precision_arg)
- {
- pointCompressionResolution_ = precision_arg;
- }
-
- /** \brief Retrieve precision of point information
- * \return precision
- */
- inline float
- getPrecision ()
- {
- return (pointCompressionResolution_);
- }
-
- /** \brief Set amount of points within point cloud to be encoded and reserve memory
- * \param pointCount_arg: amounts of points within point cloud
- */
- inline void
- setPointCount (unsigned int pointCount_arg)
- {
- pointDiffDataVector_.reserve (pointCount_arg * 3);
- }
-
- /** \brief Initialize encoding of differential point */
- void
- initializeEncoding ()
- {
- pointDiffDataVector_.clear ();
- }
-
- /** \brief Initialize decoding of differential point */
- void
- initializeDecoding ()
- {
- pointDiffDataVectorIterator_ = pointDiffDataVector_.begin ();
- }
-
- /** \brief Get reference to vector containing differential color data */
- std::vector<char>&
- getDifferentialDataVector ()
- {
- return (pointDiffDataVector_);
- }
-
- /** \brief Encode differential point information for a subset of points from point cloud
- * \param indexVector_arg indices defining a subset of points from points cloud
- * \param referencePoint_arg coordinates of reference point
- * \param inputCloud_arg input point cloud
- */
- void
- encodePoints (const typename std::vector<int>& indexVector_arg, const double* referencePoint_arg,
- PointCloudConstPtr inputCloud_arg)
- {
- std::size_t len = indexVector_arg.size ();
-
- // iterate over points within current voxel
- for (std::size_t i = 0; i < len; i++)
- {
- unsigned char diffX, diffY, diffZ;
-
- // retrieve point from cloud
- const int& idx = indexVector_arg[i];
- const PointT& idxPoint = inputCloud_arg->points[idx];
-
- // differentially encode point coordinates and truncate overflow
- diffX = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.x - referencePoint_arg[0]) / pointCompressionResolution_))));
- diffY = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.y - referencePoint_arg[1]) / pointCompressionResolution_))));
- diffZ = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.z - referencePoint_arg[2]) / pointCompressionResolution_))));
-
- // store information in differential point vector
- pointDiffDataVector_.push_back (diffX);
- pointDiffDataVector_.push_back (diffY);
- pointDiffDataVector_.push_back (diffZ);
- }
- }
-
- /** \brief Decode differential point information
- * \param outputCloud_arg output point cloud
- * \param referencePoint_arg coordinates of reference point
- * \param beginIdx_arg index indicating first point to be assigned with color information
- * \param endIdx_arg index indicating last point to be assigned with color information
- */
- void
- decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, std::size_t beginIdx_arg,
- std::size_t endIdx_arg)
- {
- assert (beginIdx_arg <= endIdx_arg);
-
- unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
-
- // iterate over points within current voxel
- for (std::size_t i = 0; i < pointCount; i++)
- {
- // retrieve differential point information
- const unsigned char& diffX = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
- const unsigned char& diffY = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
- const unsigned char& diffZ = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
-
- // retrieve point from point cloud
- PointT& point = outputCloud_arg->points[beginIdx_arg + i];
-
- // decode point position
- point.x = static_cast<float> (referencePoint_arg[0] + diffX * pointCompressionResolution_);
- point.y = static_cast<float> (referencePoint_arg[1] + diffY * pointCompressionResolution_);
- point.z = static_cast<float> (referencePoint_arg[2] + diffZ * pointCompressionResolution_);
- }
- }
-
- protected:
- /** \brief Pointer to output point cloud dataset. */
- PointCloudPtr output_;
-
- /** \brief Vector for storing differential point information */
- std::vector<char> pointDiffDataVector_;
-
- /** \brief Iterator on differential point information vector */
- std::vector<char>::const_iterator pointDiffDataVectorIterator_;
-
- /** \brief Precision of point coding*/
- float pointCompressionResolution_;
- };
- }
-}
+ return (pointCompressionResolution_);
+ }
+
+ /** \brief Set amount of points within point cloud to be encoded and reserve memory
+ * \param pointCount_arg: amounts of points within point cloud
+ */
+ inline void
+ setPointCount (unsigned int pointCount_arg)
+ {
+ pointDiffDataVector_.reserve (pointCount_arg * 3);
+ }
+
+ /** \brief Initialize encoding of differential point */
+ void
+ initializeEncoding ()
+ {
+ pointDiffDataVector_.clear ();
+ }
+
+ /** \brief Initialize decoding of differential point */
+ void
+ initializeDecoding ()
+ {
+ pointDiffDataVectorIterator_ = pointDiffDataVector_.begin ();
+ }
+
+ /** \brief Get reference to vector containing differential color data */
+ std::vector<char>&
+ getDifferentialDataVector ()
+ {
+ return (pointDiffDataVector_);
+ }
+
+ /** \brief Encode differential point information for a subset of points from point cloud
+ * \param indexVector_arg indices defining a subset of points from points cloud
+ * \param referencePoint_arg coordinates of reference point
+ * \param inputCloud_arg input point cloud
+ */
+ void
+ encodePoints (const typename std::vector<int>& indexVector_arg, const double* referencePoint_arg,
+ PointCloudConstPtr inputCloud_arg)
+ {
+ std::size_t len = indexVector_arg.size ();
+
+ // iterate over points within current voxel
+ for (std::size_t i = 0; i < len; i++)
+ {
+ unsigned char diffX, diffY, diffZ;
+
+ // retrieve point from cloud
+ const int& idx = indexVector_arg[i];
+ const PointT& idxPoint = inputCloud_arg->points[idx];
+
+ // differentially encode point coordinates and truncate overflow
+ diffX = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.x - referencePoint_arg[0]) / pointCompressionResolution_))));
+ diffY = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.y - referencePoint_arg[1]) / pointCompressionResolution_))));
+ diffZ = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.z - referencePoint_arg[2]) / pointCompressionResolution_))));
+
+ // store information in differential point vector
+ pointDiffDataVector_.push_back (diffX);
+ pointDiffDataVector_.push_back (diffY);
+ pointDiffDataVector_.push_back (diffZ);
+ }
+ }
+
+ /** \brief Decode differential point information
+ * \param outputCloud_arg output point cloud
+ * \param referencePoint_arg coordinates of reference point
+ * \param beginIdx_arg index indicating first point to be assigned with color information
+ * \param endIdx_arg index indicating last point to be assigned with color information
+ */
+ void
+ decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, std::size_t beginIdx_arg,
+ std::size_t endIdx_arg)
+ {
+ assert (beginIdx_arg <= endIdx_arg);
+
+ unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
+
+ // iterate over points within current voxel
+ for (std::size_t i = 0; i < pointCount; i++)
+ {
+ // retrieve differential point information
+ const unsigned char& diffX = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
+ const unsigned char& diffY = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
+ const unsigned char& diffZ = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
+
+ // retrieve point from point cloud
+ PointT& point = outputCloud_arg->points[beginIdx_arg + i];
+
+ // decode point position
+ point.x = static_cast<float> (referencePoint_arg[0] + diffX * pointCompressionResolution_);
+ point.y = static_cast<float> (referencePoint_arg[1] + diffY * pointCompressionResolution_);
+ point.z = static_cast<float> (referencePoint_arg[2] + diffZ * pointCompressionResolution_);
+ }
+ }
+
+ protected:
+ /** \brief Pointer to output point cloud dataset. */
+ PointCloudPtr output_;
+
+ /** \brief Vector for storing differential point information */
+ std::vector<char> pointDiffDataVector_;
+
+ /** \brief Iterator on differential point information vector */
+ std::vector<char>::const_iterator pointDiffDataVectorIterator_;
+
+ /** \brief Precision of point coding*/
+ float pointCompressionResolution_;
+};
+
+} // namespace octree
+} // namespace pcl
#define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding<T>;
+
* \param[in] p a point type
*/
template<typename PointT>
- PCL_DEPRECATED("use parameterless setInputFields<PointT>() instead")
+ PCL_DEPRECATED(
1, 12, "use parameterless setInputFields<PointT>() instead")
inline void setInputFields (const PointT p)
{
(void) p;
#pragma once
#if defined __GNUC__
-# pragma GCC system_header
+# pragma GCC system_header
#endif
//https://bugreports.qt-project.org/browse/QTBUG-22829
#ifndef Q_MOC_RUN
#include <boost/version.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/filesystem.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/inherit.hpp>
#include <boost/mpl/inherit_linearly.hpp>
* @return True if successful, false otherwise
* @warning A device must be opened and not running */
bool
- grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud);
+ grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud) const;
/** @brief Set up the Ensenso sensor and API to do 3D extrinsic calibration using the Ensenso 2D patterns
* @param[in] grid_spacing
setExtrinsicCalibration (const double euler_angle,
Eigen::Vector3d &rotation_axis,
const Eigen::Vector3d &translation,
- const std::string target = "Hand");
+ const std::string target = "Hand") const;
/** @brief Update Link node in NxLib tree with an identity matrix
* @param[in] target "Hand" or "Workspace" for example
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
-
+
#pragma once
#include <pcl/pcl_config.h>
// needed for the grabber interface / observers
#include <map>
+#include <memory>
#include <iostream>
#include <string>
#include <typeinfo>
+#include <tuple>
#include <vector>
#include <sstream>
#include <pcl/pcl_macros.h>
class PCL_EXPORTS Grabber
{
public:
+ /**
+ * \brief Default ctor
+ */
+ Grabber() = default;
+
+ /**
+ * \brief No copy ctor since Grabber can't be copied
+ */
+ Grabber(const Grabber&) = delete;
+
+ /**
+ * \brief No copy assign operator since Grabber can't be copied
+ */
+ Grabber& operator=(const Grabber&) = delete;
+
+ /**
+ * \brief Move ctor
+ */
+ Grabber(Grabber&&) = default;
+
+ /**
+ * \brief Move assign operator
+ */
+ Grabber& operator=(Grabber&&) = default;
+
/** \brief virtual destructor. */
- virtual inline ~Grabber () noexcept;
+ #if defined(_MSC_VER)
+ virtual inline ~Grabber () noexcept {}
+ #else
+ virtual inline ~Grabber () noexcept = default;
+ #endif
/** \brief registers a callback function/method to a signal with the corresponding signature
* \param[in] callback: the callback function/method
* \return Connection object, that can be used to disconnect the callback method from the signal again.
*/
template<typename T, template<typename> class FunctionT>
- PCL_DEPRECATED ("please assign the callback to a std::function.")
+ PCL_DEPRECATED (1, 12, "please assign the callback to a std::function.")
boost::signals2::connection
registerCallback (const FunctionT<T>& callback)
{
/** \brief indicates whether a signal with given parameter-type exists or not
* \return true if signal exists, false otherwise
*/
- template<typename T> bool
- providesCallback () const;
+ template<typename T> bool
+ providesCallback () const noexcept;
/** \brief For devices that are streaming, the streams are started by calling this method.
* Trigger-based devices, just trigger the device once for each call of start.
*/
- virtual void
+ virtual void
start () = 0;
/** \brief For devices that are streaming, the streams are stopped.
* This method has no effect for triggered devices.
*/
- virtual void
+ virtual void
stop () = 0;
/** \brief For devices that are streaming, stopped streams are started and running stream are stopped.
/** \brief returns the name of the concrete subclass.
* \return the name of the concrete driver.
*/
- virtual std::string
+ virtual std::string
getName () const = 0;
/** \brief Indicates whether the grabber is streaming or not. This value is not defined for triggered devices.
* \return true if grabber is running / streaming. False otherwise.
*/
- virtual bool
+ virtual bool
isRunning () const = 0;
/** \brief returns fps. 0 if trigger based. */
- virtual float
+ virtual float
getFramesPerSecond () const = 0;
protected:
virtual void
signalsChanged () { }
- template<typename T> boost::signals2::signal<T>*
- find_signal () const;
+ template<typename T> boost::signals2::signal<T>*
+ find_signal () const noexcept;
- template<typename T> int
- num_slots () const;
+ template<typename T> int
+ num_slots () const noexcept;
- template<typename T> void
+ template<typename T> void
disconnect_all_slots ();
- template<typename T> void
+ template<typename T> void
block_signal ();
-
- template<typename T> void
+
+ template<typename T> void
unblock_signal ();
-
- inline void
+
+ inline void
block_signals ();
-
- inline void
+
+ inline void
unblock_signals ();
- template<typename T> boost::signals2::signal<T>*
+ template<typename T> boost::signals2::signal<T>*
createSignal ();
- std::map<std::string, boost::signals2::signal_base*> signals_;
+ std::map<std::string, std::unique_ptr<boost::signals2::signal_base>> signals_;
std::map<std::string, std::vector<boost::signals2::connection> > connections_;
std::map<std::string, std::vector<boost::signals2::shared_connection_block> > shared_connections_;
} ;
- Grabber::~Grabber () noexcept
- {
- for (auto &signal : signals_)
- delete signal.second;
- }
-
bool
Grabber::toggle ()
{
}
template<typename T> boost::signals2::signal<T>*
- Grabber::find_signal () const
+ Grabber::find_signal () const noexcept
{
using Signal = boost::signals2::signal<T>;
- std::map<std::string, boost::signals2::signal_base*>::const_iterator signal_it = signals_.find (typeid (T).name ());
+ const auto signal_it = signals_.find (typeid (T).name ());
if (signal_it != signals_.end ())
- return (dynamic_cast<Signal*> (signal_it->second));
-
- return (NULL);
+ {
+ return (static_cast<Signal*> (signal_it->second.get ()));
+ }
+ return nullptr;
}
template<typename T> void
Grabber::disconnect_all_slots ()
{
- using Signal = boost::signals2::signal<T>;
-
- if (signals_.find (typeid (T).name ()) != signals_.end ())
+ const auto signal = find_signal<T> ();
+ if (signal != nullptr)
{
- Signal* signal = dynamic_cast<Signal*> (signals_[typeid (T).name ()]);
signal->disconnect_all_slots ();
}
}
}
template<typename T> int
- Grabber::num_slots () const
+ Grabber::num_slots () const noexcept
{
- using Signal = boost::signals2::signal<T>;
-
- // see if we have a signal for this type
- std::map<std::string, boost::signals2::signal_base*>::const_iterator signal_it = signals_.find (typeid (T).name ());
- if (signal_it != signals_.end ())
+ const auto signal = find_signal<T> ();
+ if (signal != nullptr)
{
- Signal* signal = dynamic_cast<Signal*> (signal_it->second);
- return (static_cast<int> (signal->num_slots ()));
+ return static_cast<int> (signal->num_slots ());
}
- return (0);
+ return 0;
}
template<typename T> boost::signals2::signal<T>*
Grabber::createSignal ()
{
using Signal = boost::signals2::signal<T>;
-
- if (signals_.find (typeid (T).name ()) == signals_.end ())
+ using Base = boost::signals2::signal_base;
+ // DefferedPtr serves 2 purposes:
+ // * allows MSVC to copy it around, can't do that with unique_ptr<T>
+ // * performs dynamic allocation only when required. If the key is found, this
+ // struct is a no-op, otherwise it allocates when implicit conversion operator
+ // is called inside emplace/try_emplace
+ struct DefferedPtr {
+ operator std::unique_ptr<Base>() const { return std::make_unique<Signal>(); }
+ };
+ // TODO: remove later for C++17 features: structured bindings and try_emplace
+ #ifdef __cpp_structured_bindings
+ const auto [iterator, success] =
+ #else
+ typename decltype(signals_)::const_iterator iterator;
+ bool success;
+ std::tie (iterator, success) =
+ #endif
+
+ #ifdef __cpp_lib_map_try_emplace
+ signals_.try_emplace (
+ #else
+ signals_.emplace (
+ #endif
+ std::string (typeid (T).name ()), DefferedPtr ());
+ if (!success)
{
- Signal* signal = new Signal ();
- signals_[typeid (T).name ()] = signal;
- return (signal);
+ return nullptr;
}
- return (nullptr);
+ return static_cast<Signal*> (iterator->second.get ());
}
template<typename T> boost::signals2::connection
Grabber::registerCallback (const std::function<T> & callback)
{
- using Signal = boost::signals2::signal<T>;
- if (signals_.find (typeid (T).name ()) == signals_.end ())
+ const auto signal = find_signal<T> ();
+ if (signal == nullptr)
{
std::stringstream sstream;
PCL_THROW_EXCEPTION (pcl::IOException, "[" << getName () << "] " << sstream.str ());
//return (boost::signals2::connection ());
}
- Signal* signal = dynamic_cast<Signal*> (signals_[typeid (T).name ()]);
boost::signals2::connection ret = signal->connect (callback);
connections_[typeid (T).name ()].push_back (ret);
}
template<typename T> bool
- Grabber::providesCallback () const
+ Grabber::providesCallback () const noexcept
{
- if (signals_.find (typeid (T).name ()) == signals_.end ())
- return (false);
- return (true);
+ return find_signal<T> ();
}
} // namespace
*/
using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba = void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &, float, float);
- using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb PCL_DEPRECATED("use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead")
+ using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb PCL_DEPRECATED(1, 12, "use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead")
= sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba;
/** \brief Signal used for a single sector
*/
using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba = void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &);
- using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb PCL_DEPRECATED("use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead")
+ using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb PCL_DEPRECATED(1, 12, "use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead")
= sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba;
/** \brief Constructor taking an optional path to an HDL corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368]
computeXYZI (pcl::PointXYZI& pointXYZI,
std::uint16_t azimuth,
HDLLaserReturn laserReturn,
- HDLLaserCorrection correction);
+ HDLLaserCorrection correction) const;
private:
* POSSIBILITY OF SUCH DAMAGE.
*
*/
-
-#pragma once
-#include <chrono>
+#pragma once
-#include <pcl/pcl_config.h>
-#include <pcl/pcl_exports.h>
#include <pcl/io/boost.h>
-
#include <pcl/io/image_metadata_wrapper.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_config.h>
+#include <pcl/pcl_macros.h>
+
+#include <chrono>
namespace pcl
{
namespace io
- {
+ {
/**
* @brief Image interface class providing an interface to fill a RGB or Grayscale image buffer.
} // namespace
}
+
#pragma once
+#include <pcl/conversions.h>
+#include <pcl/memory.h>
#include <pcl/pcl_config.h>
+#include <pcl/common/time_trigger.h>
#include <pcl/io/grabber.h>
#include <pcl/io/file_grabber.h>
-#include <pcl/common/time_trigger.h>
-#include <pcl/conversions.h>
-
-#include <boost/shared_ptr.hpp>
#include <string>
#include <vector>
+
namespace pcl
{
/** \brief Base class for Image file grabber.
*/
ImageGrabberBase (const std::vector<std::string>& depth_image_files, float frames_per_second, bool repeat);
- /** \brief Copy constructor.
- * \param[in] src the Image Grabber base object to copy into this
- */
- ImageGrabberBase (const ImageGrabberBase &src) : impl_ ()
- {
- *this = src;
- }
-
- /** \brief Copy operator.
- * \param[in] src the Image Grabber base object to copy into this
- */
- ImageGrabberBase&
- operator = (const ImageGrabberBase &src)
- {
- impl_ = src.impl_;
- return (*this);
- }
-
/** \brief Virtual destructor. */
~ImageGrabberBase () noexcept;
/** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */
- void
+ void
start () override;
-
+
/** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */
- void
+ void
stop () override;
-
+
/** \brief Triggers a callback with new data */
- virtual void
+ virtual void
trigger ();
/** \brief whether the grabber is started (publishing) or not.
* \return true only if publishing.
*/
- bool
+ bool
isRunning () const override;
-
+
/** \return The name of the grabber */
- std::string
+ std::string
getName () const override;
-
+
/** \brief Rewinds to the first PCD file in the list.*/
- virtual void
+ virtual void
rewind ();
/** \brief Returns the frames_per_second. 0 if grabber is trigger-based */
- float
+ float
getFramesPerSecond () const override;
/** \brief Returns whether the repeat flag is on */
- bool
+ bool
isRepeatOn () const;
/** \brief Returns if the last frame is reached */
std::string
getCurrentDepthFileName () const;
- /** \brief Returns the filename of the previous indexed file
+ /** \brief Returns the filename of the previous indexed file
* SDM: adding this back in, but is this useful, or confusing? */
std::string
getPrevDepthFileName () const;
void
setRGBImageFiles (const std::vector<std::string>& rgb_image_files);
- /** \brief Define custom focal length and center pixel. This will override ANY other setting of parameters for the duration of the grabber's life, whether by factory defaults or explicitly read from a frame_[timestamp].xml file.
+ /** \brief Define custom focal length and center pixel. This will override ANY other setting of parameters for the duration of the grabber's life, whether by factory defaults or explicitly read from a frame_[timestamp].xml file.
* \param[in] focal_length_x Horizontal focal length (fx)
* \param[in] focal_length_y Vertical focal length (fy)
* \param[in] principal_point_x Horizontal coordinates of the principal point (cx)
* \param[in] principal_point_y Vertical coordinates of the principal point (cy)
*/
virtual void
- setCameraIntrinsics (const double focal_length_x,
- const double focal_length_y,
- const double principal_point_x,
+ setCameraIntrinsics (const double focal_length_x,
+ const double focal_length_y,
+ const double principal_point_x,
const double principal_point_y);
-
+
/** \brief Get the current focal length and center pixel. If the intrinsics have been manually set with setCameraIntrinsics, this will return those values. Else, if start () has been called and the grabber has found a frame_[timestamp].xml file, this will return the most recent values read. Else, returns factory defaults.
* \param[out] focal_length_x Horizontal focal length (fx)
* \param[out] focal_length_y Vertical focal length (fy)
* \param[out] principal_point_y Vertical coordinates of the principal point (cy)
*/
virtual void
- getCameraIntrinsics (double &focal_length_x,
- double &focal_length_y,
- double &principal_point_x,
+ getCameraIntrinsics (double &focal_length_x,
+ double &focal_length_y,
+ double &principal_point_x,
double &principal_point_y) const;
/** \brief Define the units the depth data is stored in.
* Defaults to mm (0.001), meaning a brightness of 1000 corresponds to 1 m*/
void
setDepthImageUnits (float units);
-
+
/** \brief Set the number of threads, if we wish to use OpenMP for quicker cloud population.
* Note that for a standard (< 4 core) machine this is unlikely to yield a drastic speedup.*/
void
*/
std::size_t
numFrames () const;
-
+
/** \brief Gets the cloud in ROS form at location idx */
bool
getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const;
private:
- virtual void
+ virtual void
publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;
using Ptr = shared_ptr<ImageGrabber>;
using ConstPtr = shared_ptr<const ImageGrabber>;
- ImageGrabber (const std::string& dir,
- float frames_per_second = 0,
- bool repeat = false,
+ ImageGrabber (const std::string& dir,
+ float frames_per_second = 0,
+ bool repeat = false,
bool pclzf_mode = false);
- ImageGrabber (const std::string& depth_dir,
- const std::string& rgb_dir,
- float frames_per_second = 0,
+ ImageGrabber (const std::string& depth_dir,
+ const std::string& rgb_dir,
+ float frames_per_second = 0,
bool repeat = false);
- ImageGrabber (const std::vector<std::string>& depth_image_files,
- float frames_per_second = 0,
+ ImageGrabber (const std::vector<std::string>& depth_image_files,
+ float frames_per_second = 0,
bool repeat = false);
-
+
/** \brief Empty destructor */
~ImageGrabber () noexcept {}
-
+
// Inherited from FileGrabber
const typename pcl::PointCloud<PointT>::ConstPtr
operator[] (std::size_t idx) const override;
size () const override;
protected:
- void
+ void
publish (const pcl::PCLPointCloud2& blob,
- const Eigen::Vector4f& origin,
+ const Eigen::Vector4f& origin,
const Eigen::Quaternionf& orientation) const override;
boost::signals2::signal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>* signal_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
- ImageGrabber<PointT>::ImageGrabber (const std::string& dir,
- float frames_per_second,
- bool repeat,
+ ImageGrabber<PointT>::ImageGrabber (const std::string& dir,
+ float frames_per_second,
+ bool repeat,
bool pclzf_mode)
: ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode)
{
signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
}
-
+
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
- ImageGrabber<PointT>::ImageGrabber (const std::string& depth_dir,
- const std::string& rgb_dir,
- float frames_per_second,
+ ImageGrabber<PointT>::ImageGrabber (const std::string& depth_dir,
+ const std::string& rgb_dir,
+ float frames_per_second,
bool repeat)
: ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat)
{
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
- ImageGrabber<PointT>::ImageGrabber (const std::vector<std::string>& depth_image_files,
- float frames_per_second,
+ ImageGrabber<PointT>::ImageGrabber (const std::vector<std::string>& depth_image_files,
+ float frames_per_second,
bool repeat)
: ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ ()
{
#include <chrono>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/io/boost.h>
/*
* Software License Agreement (BSD License)
- *
+ *
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2012, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
* Copyright (c) 2014, respective authors.
- *
+ *
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
- *
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_config.h>
-#include <pcl/pcl_macros.h>
namespace pcl
{
} // namespace
}
+
*
*/
+
#ifndef PCL_IO_ASCII_IO_HPP_
#define PCL_IO_ASCII_IO_HPP_
+
+namespace pcl
+{
+
template<typename PointT> void
-pcl::ASCIIReader::setInputFields ()
+ASCIIReader::setInputFields ()
{
fields_ = pcl::getFields<PointT> ();
}
}
+} // namespace pcl
#endif //PCL_IO_ASCII_IO_HPP_
#include <pcl/pcl_macros.h>
+
template <typename T>
struct buffer_traits
{
static bool is_invalid (double value) { return std::isnan (value); };
};
+
+namespace pcl
+{
+
+namespace io
+{
+
template <typename T>
-pcl::io::Buffer<T>::Buffer (std::size_t size)
+Buffer<T>::Buffer (std::size_t size)
: size_ (size)
{
}
template <typename T>
-pcl::io::Buffer<T>::~Buffer ()
+Buffer<T>::~Buffer ()
{
}
template <typename T>
-pcl::io::SingleBuffer<T>::SingleBuffer (std::size_t size)
+SingleBuffer<T>::SingleBuffer (std::size_t size)
: Buffer<T> (size)
, data_ (size, buffer_traits<T>::invalid ())
{
}
template <typename T>
-pcl::io::SingleBuffer<T>::~SingleBuffer ()
+SingleBuffer<T>::~SingleBuffer ()
{
}
template <typename T> T
-pcl::io::SingleBuffer<T>::operator[] (std::size_t idx) const
+SingleBuffer<T>::operator[] (std::size_t idx) const
{
assert (idx < size_);
return (data_[idx]);
}
template <typename T> void
-pcl::io::SingleBuffer<T>::push (std::vector<T>& data)
+SingleBuffer<T>::push (std::vector<T>& data)
{
assert (data.size () == size_);
std::lock_guard<std::mutex> lock (data_mutex_);
}
template <typename T>
-pcl::io::MedianBuffer<T>::MedianBuffer (std::size_t size,
- unsigned char window_size)
+MedianBuffer<T>::MedianBuffer (std::size_t size,
+ unsigned char window_size)
: Buffer<T> (size)
, window_size_ (window_size)
, midpoint_ (window_size_ / 2)
}
template <typename T>
-pcl::io::MedianBuffer<T>::~MedianBuffer ()
+MedianBuffer<T>::~MedianBuffer ()
{
}
template <typename T> T
-pcl::io::MedianBuffer<T>::operator[] (std::size_t idx) const
+MedianBuffer<T>::operator[] (std::size_t idx) const
{
assert (idx < size_);
int midpoint = (window_size_ - data_invalid_count_[idx]) / 2;
}
template <typename T> void
-pcl::io::MedianBuffer<T>::push (std::vector<T>& data)
+MedianBuffer<T>::push (std::vector<T>& data)
{
assert (data.size () == size_);
std::lock_guard<std::mutex> lock (data_mutex_);
}
template <typename T> int
-pcl::io::MedianBuffer<T>::compare (T a, T b)
+MedianBuffer<T>::compare (T a, T b)
{
bool a_is_invalid = buffer_traits<T>::is_invalid (a);
bool b_is_invalid = buffer_traits<T>::is_invalid (b);
}
template <typename T>
-pcl::io::AverageBuffer<T>::AverageBuffer (std::size_t size,
- unsigned char window_size)
+AverageBuffer<T>::AverageBuffer (std::size_t size,
+ unsigned char window_size)
: Buffer<T> (size)
, window_size_ (window_size)
, data_current_idx_ (window_size_ - 1)
}
template <typename T>
-pcl::io::AverageBuffer<T>::~AverageBuffer ()
+AverageBuffer<T>::~AverageBuffer ()
{
}
template <typename T> T
-pcl::io::AverageBuffer<T>::operator[] (std::size_t idx) const
+AverageBuffer<T>::operator[] (std::size_t idx) const
{
assert (idx < size_);
if (data_invalid_count_[idx] == window_size_)
}
template <typename T> void
-pcl::io::AverageBuffer<T>::push (std::vector<T>& data)
+AverageBuffer<T>::push (std::vector<T>& data)
{
assert (data.size () == size_);
std::lock_guard<std::mutex> lock (data_mutex_);
data.clear ();
}
+} // namespace io
+} // namespace pcl
+
#endif /* PCL_IO_IMPL_BUFFERS_HPP */
#define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
-//////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace io
+{
+
template <typename PointT> bool
-pcl::io::LZFDepth16ImageReader::read (
+LZFDepth16ImageReader::read (
const std::string &filename, pcl::PointCloud<PointT> &cloud)
{
std::uint32_t uncompressed_size;
}
pt.z = static_cast<float> (val * z_multiplication_factor_);
- pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x))
+ pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x))
* pt.z * static_cast<float> (constant_x);
- pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y))
+ pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y))
* pt.z * static_cast<float> (constant_y);
}
}
cloud.sensor_orientation_.z () = 0.0f;
return (true);
}
-
-///////////////////////////////////////////////////////////////////////////////
+
+
template <typename PointT> bool
-pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename,
- pcl::PointCloud<PointT> &cloud,
- unsigned int num_threads)
+LZFDepth16ImageReader::readOMP (const std::string &filename,
+ pcl::PointCloud<PointT> &cloud,
+ unsigned int num_threads)
{
std::uint32_t uncompressed_size;
std::vector<char> compressed_data;
double constant_x = 1.0 / parameters_.focal_length_x,
constant_y = 1.0 / parameters_.focal_length_y;
#ifdef _OPENMP
-#pragma omp parallel for num_threads (num_threads)
+#pragma omp parallel for \
+ default(none) \
+ shared(cloud, constant_x, constant_y, uncompressed_data) \
+ num_threads(num_threads)
#else
(void) num_threads; // suppress warning if OMP is not present
#endif
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
if (cloud.is_dense)
{
-#ifdef _OPENMP
#pragma omp critical
-#endif
- {
- if (cloud.is_dense)
- cloud.is_dense = false;
- }
+ {
+ if (cloud.is_dense)
+ cloud.is_dense = false;
+ }
}
continue;
}
* pt.z * static_cast<float> (constant_x);
pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y))
* pt.z * static_cast<float> (constant_y);
-
}
+
cloud.sensor_origin_.setZero ();
cloud.sensor_orientation_.w () = 1.0f;
cloud.sensor_orientation_.x () = 0.0f;
cloud.sensor_orientation_.y () = 0.0f;
cloud.sensor_orientation_.z () = 0.0f;
return (true);
-
}
-//////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::io::LZFRGB24ImageReader::read (
+LZFRGB24ImageReader::read (
const std::string &filename, pcl::PointCloud<PointT> &cloud)
{
std::uint32_t uncompressed_size;
return (true);
}
-//////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::io::LZFRGB24ImageReader::readOMP (
+LZFRGB24ImageReader::readOMP (
const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
{
std::uint32_t uncompressed_size;
unsigned char *color_b = reinterpret_cast<unsigned char*> (&uncompressed_data[2 * getWidth () * getHeight ()]);
#ifdef _OPENMP
-#pragma omp parallel for num_threads (num_threads)
+#pragma omp parallel for \
+ default(none) \
+ shared(cloud, color_b, color_g, color_r) \
+ num_threads(num_threads)
#else
(void) num_threads; // suppress warning if OMP is not present
#endif//_OPENMP
return (true);
}
-//////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::io::LZFYUV422ImageReader::read (
+LZFYUV422ImageReader::read (
const std::string &filename, pcl::PointCloud<PointT> &cloud)
{
std::uint32_t uncompressed_size;
unsigned char *color_u = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
-
+
int y_idx = 0;
for (int i = 0; i < wh2; ++i, y_idx += 2)
{
return (true);
}
-//////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::io::LZFYUV422ImageReader::readOMP (
+LZFYUV422ImageReader::readOMP (
const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
{
std::uint32_t uncompressed_size;
unsigned char *color_u = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
-
+
#ifdef _OPENMP
-#pragma omp parallel for num_threads (num_threads)
+#pragma omp parallel for \
+ default(none) \
+ shared(cloud, color_u, color_v, color_y, wh2) \
+ num_threads(num_threads)
#else
(void) num_threads; //suppress warning if OMP is not present
#endif//_OPENMP
return (true);
}
-//////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::io::LZFBayer8ImageReader::read (
+LZFBayer8ImageReader::read (
const std::string &filename, pcl::PointCloud<PointT> &cloud)
{
std::uint32_t uncompressed_size;
// Convert Bayer8 to RGB24
std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
- pcl::io::DeBayer i;
- i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
- static_cast<unsigned char*> (&rgb_buffer[0]),
+ DeBayer i;
+ i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
+ static_cast<unsigned char*> (&rgb_buffer[0]),
getWidth (), getHeight ());
// Copy to PointT
cloud.width = getWidth ();
return (true);
}
-//////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::io::LZFBayer8ImageReader::readOMP (
+LZFBayer8ImageReader::readOMP (
const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
{
std::uint32_t uncompressed_size;
// Convert Bayer8 to RGB24
std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
- pcl::io::DeBayer i;
- i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
- static_cast<unsigned char*> (&rgb_buffer[0]),
+ DeBayer i;
+ i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
+ static_cast<unsigned char*> (&rgb_buffer[0]),
getWidth (), getHeight ());
// Copy to PointT
cloud.width = getWidth ();
cloud.height = getHeight ();
cloud.resize (getWidth () * getHeight ());
#ifdef _OPENMP
-#pragma omp parallel for num_threads (num_threads)
+#pragma omp parallel for \
+ default(none) \
+ num_threads(num_threads)
#else
(void) num_threads; //suppress warning if OMP is not present
#endif//_OPENMP
return (true);
}
+} // namespace io
+} // namespace pcl
+
#endif //#ifndef PCL_LZF_IMAGE_IO_HPP_
*
*/
-#ifndef PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
-#define PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
+#pragma once
#include <set>
#include <map>
#include <pcl/common/io.h>
#include <pcl/common/colors.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
return (true);
}
-#endif // PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
-
*
*/
-#ifndef PCL_IO_VTK_IO_IMPL_H_
-#define PCL_IO_VTK_IO_IMPL_H_
+#pragma once
// PCL
#include <pcl/common/io.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
// VTK
// Ignore warnings in the above headers
#undef GetTupleValue
#endif
-#endif //#ifndef PCL_IO_VTK_IO_H_
-
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/TextureMesh.h>
#include <pcl/PolygonMesh.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_config.h>
#include <pcl/pcl_macros.h>
/** \brief For devices that are streaming, the streams are started by calling this method.
* Trigger-based devices, just trigger the device once for each call of start.
*/
- void
+ void
start () override;
/** \brief For devices that are streaming, the streams are stopped.
* This method has no effect for triggered devices.
*/
- void
+ void
stop () override;
/** \brief returns the name of the concrete subclass.
* \return the name of the concrete driver.
*/
- std::string
+ std::string
getName () const override;
/** \brief Indicates whether the grabber is streaming or not. This value is not defined for triggered devices.
* \return true if grabber is running / streaming. False otherwise.
*/
- bool
+ bool
isRunning () const override;
/** \brief returns the frames pre second. 0 if it is trigger based. */
- float
+ float
getFramesPerSecond () const override;
/** \brief Check if there is any data left in the ONI file to process. */
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
-#include "openni.h"
-#include "pcl/io/openni2/openni2_video_mode.h"
-#include "pcl/io/io_exception.h"
-
-#include <boost/shared_ptr.hpp>
// Template frame wrappers
#include <pcl/io/image.h>
#include <pcl/io/image_depth.h>
#include <pcl/io/image_ir.h>
+#include <pcl/io/io_exception.h>
+#include <pcl/io/openni2/openni2_video_mode.h>
+
+#include "openni.h"
+
#include <cstdint>
#include <functional>
#include <memory>
#include <vector>
-
namespace openni
{
class Device;
#pragma once
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <pcl/io/openni2/openni2_device.h>
#include <pcl/io/openni2/openni2_device_info.h>
getDevice (const std::string& device_URI);
OpenNI2Device::Ptr
- getDeviceByIndex (int index);
+ getDeviceByIndex (int index) const;
OpenNI2Device::Ptr
getFileDevice (const std::string& path);
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_config.h>
#include <pcl/pcl_macros.h>
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include "openni.h"
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include "openni_exception.h"
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include "openni_device.h"
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include "openni_device.h"
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include "openni_device.h"
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include "openni_device.h"
#include "openni.h"
#include "openni_exception.h"
#include "openni_device.h"
-#include <pcl/io/boost.h>
+
+#include <pcl/memory.h> // for pcl::weak_ptr
#include <pcl/pcl_macros.h>
#include <map>
std::shared_ptr<xn::NodeInfo> image_node;
std::shared_ptr<xn::NodeInfo> depth_node;
std::shared_ptr<xn::NodeInfo> ir_node;
- boost::weak_ptr<OpenNIDevice> device;
+ pcl::weak_ptr<OpenNIDevice> device;
} ;
OpenNIDriver ();
{
return static_cast<unsigned> (device_context_.size ());
}
-} // namespace
+
+} // namespace openni_wrapper
+
#endif
+
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include <pcl/pcl_exports.h>
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include <pcl/pcl_macros.h>
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include "openni_image.h"
#pragma once
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include <pcl/pcl_macros.h>
#define __OPENNI_IR_IMAGE__
#include <pcl/pcl_macros.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include "openni.h"
#include "openni_exception.h"
#include <pcl/io/boost.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_config.h>
#include <pcl/pcl_macros.h>
getName () const override;
/** \brief Obtain the number of frames per second (FPS). */
- float
+ float
getFramesPerSecond () const override;
- /** \brief Get a boost shared pointer to the \ref pcl::openni_wrapper::OpenNIDevice object. */
+ /** \brief Get a pcl::shared pointer to the openni_wrapper::OpenNIDevice object. */
inline openni_wrapper::OpenNIDevice::Ptr
getDevice () const;
* and the grabber will use the default values from the camera instead.
*/
inline void
- setRGBCameraIntrinsics (const double rgb_focal_length_x,
- const double rgb_focal_length_y,
+ setRGBCameraIntrinsics (const double rgb_focal_length_x,
+ const double rgb_focal_length_y,
const double rgb_principal_point_x,
const double rgb_principal_point_y)
{
rgb_principal_point_x_ = rgb_principal_point_x;
rgb_principal_point_y_ = rgb_principal_point_y;
}
-
+
/** \brief Get the RGB camera parameters (fx, fy, cx, cy)
* \param[out] rgb_focal_length_x the RGB focal length (fx)
* \param[out] rgb_focal_length_y the RGB focal length (fy)
* \param[out] rgb_principal_point_y the RGB principal point (cy)
*/
inline void
- getRGBCameraIntrinsics (double &rgb_focal_length_x,
- double &rgb_focal_length_y,
+ getRGBCameraIntrinsics (double &rgb_focal_length_x,
+ double &rgb_focal_length_y,
double &rgb_principal_point_x,
double &rgb_principal_point_y) const
{
rgb_focal_length_x = rgb_focal_length_x_;
rgb_focal_length_y = rgb_focal_length_y_;
}
-
+
/** \brief Set the Depth camera parameters (fx, fy, cx, cy)
* \param[in] depth_focal_length_x the Depth focal length (fx)
* \param[in] depth_focal_length_y the Depth focal length (fy)
* and the grabber will use the default values from the camera instead.
*/
inline void
- setDepthCameraIntrinsics (const double depth_focal_length_x,
- const double depth_focal_length_y,
+ setDepthCameraIntrinsics (const double depth_focal_length_x,
+ const double depth_focal_length_y,
const double depth_principal_point_x,
const double depth_principal_point_y)
{
depth_principal_point_x_ = depth_principal_point_x;
depth_principal_point_y_ = depth_principal_point_y;
}
-
+
/** \brief Get the Depth camera parameters (fx, fy, cx, cy)
* \param[out] depth_focal_length_x the Depth focal length (fx)
* \param[out] depth_focal_length_y the Depth focal length (fy)
* \param[out] depth_principal_point_y the Depth principal point (cy)
*/
inline void
- getDepthCameraIntrinsics (double &depth_focal_length_x,
- double &depth_focal_length_y,
+ getDepthCameraIntrinsics (double &depth_focal_length_x,
+ double &depth_focal_length_y,
double &depth_principal_point_x,
double &depth_principal_point_y) const
{
{
depth_focal_length_x_ = depth_focal_length_y_ = depth_focal_length;
}
-
+
/** \brief Set the Depth image focal length
* \param[in] depth_focal_length_x the Depth focal length (fx)
unsigned image_height_;
unsigned depth_width_;
unsigned depth_height_;
-
+
bool image_required_;
bool depth_required_;
bool ir_required_;
#include <pcl/io/file_grabber.h>
#include <pcl/common/time_trigger.h>
#include <pcl/conversions.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include <pcl/io/openni_camera/openni_image.h>
*/
class PCL_EXPORTS PCDGrabberBase : public Grabber
{
- public:
+ public:
/** \brief Constructor taking just one PCD file or one TAR file containing multiple PCD files.
* \param[in] pcd_file path to the PCD file
* \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
*/
PCDGrabberBase (const std::vector<std::string>& pcd_files, float frames_per_second, bool repeat);
- /** \brief Copy constructor.
- * \param[in] src the PCD Grabber base object to copy into this
- */
- PCDGrabberBase (const PCDGrabberBase &src) : impl_ ()
- {
- *this = src;
- }
-
- /** \brief Copy operator.
- * \param[in] src the PCD Grabber base object to copy into this
- */
- PCDGrabberBase&
- operator = (const PCDGrabberBase &src)
- {
- impl_ = src.impl_;
- return (*this);
- }
-
/** \brief Virtual destructor. */
~PCDGrabberBase () noexcept;
/** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */
- void
+ void
start () override;
-
+
/** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */
- void
+ void
stop () override;
-
+
/** \brief Triggers a callback with new data */
- virtual void
+ virtual void
trigger ();
/** \brief Indicates whether the grabber is streaming or not.
* \return true if grabber is started and hasn't run out of PCD files.
*/
- bool
+ bool
isRunning () const override;
-
+
/** \return The name of the grabber */
- std::string
+ std::string
getName () const override;
-
+
/** \brief Rewinds to the first PCD file in the list.*/
- virtual void
+ virtual void
rewind ();
/** \brief Returns the frames_per_second. 0 if grabber is trigger-based */
- float
+ float
getFramesPerSecond () const override;
/** \brief Returns whether the repeat flag is on */
- bool
+ bool
isRepeatOn () const;
-
+
/** \brief Get cloud (in ROS form) at a particular location */
bool
- getCloudAt (std::size_t idx,
+ getCloudAt (std::size_t idx,
pcl::PCLPointCloud2 &blob,
- Eigen::Vector4f &origin,
+ Eigen::Vector4f &origin,
Eigen::Quaternionf &orientation) const;
/** \brief Returns the size */
numFrames () const;
private:
- virtual void
+ virtual void
publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const = 0;
// to separate and hide the implementation from interface: PIMPL
PCDGrabber (const std::string& pcd_path, float frames_per_second = 0, bool repeat = false);
PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second = 0, bool repeat = false);
-
+
/** \brief Virtual destructor. */
~PCDGrabber () noexcept
{
stop ();
}
-
+
// Inherited from FileGrabber
const typename pcl::PointCloud<PointT>::ConstPtr
operator[] (std::size_t idx) const override;
size () const override;
protected:
- void
+ void
publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const override;
-
+
boost::signals2::signal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>* signal_;
boost::signals2::signal<void (const std::string&)>* file_name_signal_;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename PointT> void
+ template<typename PointT> void
PCDGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const
{
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
openni_wrapper::Image::Ptr image (new openni_wrapper::ImageRGB24 (image_meta_data));
if (image_signal_->num_slots() > 0)
image_signal_->operator()(image);
-
+
if (image_depth_image_signal_->num_slots() > 0)
image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f);
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/io/file_io.h>
* - HEIGHT ...
* - POINTS ...
* - DATA ascii/binary
- *
+ *
* Everything that follows \b DATA is interpreted as data points and
* will be read accordingly.
*
* \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present)
* \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present)
* \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7)
- * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed)
+ * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed)
* \param[out] data_idx the offset of cloud data within the file
*
* \return
* * < 0 (-1) on error
* * == 0 on success
*/
- int
+ int
readHeader (std::istream &binary_istream, pcl::PCLPointCloud2 &cloud,
Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version,
int &data_type, unsigned int &data_idx);
- /** \brief Read a point cloud data header from a PCD file.
+ /** \brief Read a point cloud data header from a PCD file.
*
* Load only the meta information (number of points, their types, etc),
* and not the points themselves, from a given PCD file. Useful for fast
* \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present)
* \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present)
* \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7)
- * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed)
+ * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed)
* \param[out] data_idx the offset of cloud data within the file
* \param[in] offset the offset of where to expect the PCD Header in the
* file (optional parameter). One usage example for setting the offset
* * < 0 (-1) on error
* * == 0 on success
*/
- int
+ int
readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version,
int &data_type, unsigned int &data_idx, const int offset = 0) override;
- /** \brief Read a point cloud data header from a PCD file.
+ /** \brief Read a point cloud data header from a PCD file.
*
* Load only the meta information (number of points, their types, etc),
* and not the points themselves, from a given PCD file. Useful for fast
* * < 0 (-1) on error
* * == 0 on success
*/
- int
+ int
readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0);
- /** \brief Read the point cloud data (body) from a PCD stream.
+ /** \brief Read the point cloud data (body) from a PCD stream.
*
* Reads the cloud points from a text-formatted stream. For use after
* readHeader(), when the resulting data_type == 0.
int
readBodyASCII (std::istream &stream, pcl::PCLPointCloud2 &cloud, int pcd_version);
- /** \brief Read the point cloud data (body) from a block of memory.
+ /** \brief Read the point cloud data (body) from a block of memory.
*
* Reads the cloud points from a binary-formatted memory block. For use
* after readHeader(), when the resulting data_type is nonzero.
* * < 0 (-1) on error
* * == 0 on success
*/
- int
+ int
read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset = 0) override;
/** \brief Read a point cloud data from a PCD (PCD_V6) and store it into a pcl/PCLPointCloud2.
- *
+ *
* \note This function is provided for backwards compatibility only and
* it can only read PCD_V6 files correctly, as pcl::PCLPointCloud2
- * does not contain a sensor origin/orientation. Reading any file
- * > PCD_V6 will generate a warning.
+ * does not contain a sensor origin/orientation. Reading any file
+ * > PCD_V6 will generate a warning.
*
* \param[in] file_name the name of the file containing the actual PointCloud data
* \param[out] cloud the resultant PointCloud message read from disk
* * < 0 (-1) on error
* * == 0 on success
*/
- int
+ int
read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0);
/** \brief Read a point cloud data from any PCD file, and convert it to the given template format.
{
pcl::PCLPointCloud2 blob;
int pcd_version;
- int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_,
+ int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_,
pcd_version, offset);
// If no error, convert the data
PCDWriter() : map_synchronization_(false) {}
~PCDWriter() {}
- /** \brief Set whether mmap() synchornization via msync() is desired before munmap() calls.
+ /** \brief Set whether mmap() synchornization via msync() is desired before munmap() calls.
* Setting this to true could prevent NFS data loss (see
* http://www.pcl-developers.org/PCD-IO-consistency-on-NFS-msync-needed-td4885942.html).
* Default: false
*/
std::string
generateHeaderBinary (const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin,
+ const Eigen::Vector4f &origin,
const Eigen::Quaternionf &orientation);
/** \brief Generate the header of a BINARY_COMPRESSED PCD file format
int
generateHeaderBinaryCompressed (std::ostream &os,
const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin,
+ const Eigen::Vector4f &origin,
const Eigen::Quaternionf &orientation);
/** \brief Generate the header of a BINARY_COMPRESSED PCD file format
- * \param[out] os the stream into which to write the header
* \param[in] cloud the point cloud data message
* \param[in] origin the sensor acquisition origin
* \param[in] orientation the sensor acquisition orientation
*/
std::string
generateHeaderBinaryCompressed (const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin,
+ const Eigen::Vector4f &origin,
const Eigen::Quaternionf &orientation);
/** \brief Generate the header of a PCD file format
*/
std::string
generateHeaderASCII (const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin,
+ const Eigen::Vector4f &origin,
const Eigen::Quaternionf &orientation);
/** \brief Generate the header of a PCD file format
* By default, nr_points is set to INTMAX, and the data in the header is used instead.
*/
template <typename PointT> static std::string
- generateHeader (const pcl::PointCloud<PointT> &cloud,
+ generateHeader (const pcl::PointCloud<PointT> &cloud,
const int nr_points = std::numeric_limits<int>::max ());
/** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format
*
* As an intermediary solution, precision 8 is used, which guarantees lossless storage for RGB.
*/
- int
+ int
writeASCII (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+ const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
const int precision = 8);
* \param[in] origin the sensor acquisition origin
* \param[in] orientation the sensor acquisition orientation
*/
- int
+ int
writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+ const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ());
/** \brief Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format
* (-2) if the input cloud is too large for the file format
* 0 on success
*/
- int
+ int
writeBinaryCompressed (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+ const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ());
/** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY_COMPRESSED format
*/
inline int
write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+ const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
const bool binary = false) override
{
/** \brief Save point cloud data to a PCD file containing n-D points
* \param[in] file_name the output file name
* \param[in] cloud the point cloud data message (boost shared pointer)
- * \param[in] binary set to true if the file is to be written in a binary PCD format,
+ * \param[in] binary set to true if the file is to be written in a binary PCD format,
* false (default) for ASCII
* \param[in] origin the sensor acquisition origin
* \param[in] orientation the sensor acquisition orientation
*/
inline int
write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud,
- const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+ const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
const bool binary = false)
{
* \param[in] file_name the output file name
* \param[in] cloud the point cloud data message
*/
- template <typename PointT> int
- writeBinary (const std::string &file_name,
+ template <typename PointT> int
+ writeBinary (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud);
/** \brief Save point cloud data to a binary comprssed PCD file
* (-2) if the input cloud is too large for the file format
* 0 on success
*/
- template <typename PointT> int
- writeBinaryCompressed (const std::string &file_name,
+ template <typename PointT> int
+ writeBinaryCompressed (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud);
/** \brief Save point cloud data to a PCD file containing n-D points, in BINARY format
* \param[in] cloud the point cloud data message
* \param[in] indices the set of point indices that we want written to disk
*/
- template <typename PointT> int
- writeBinary (const std::string &file_name,
- const pcl::PointCloud<PointT> &cloud,
+ template <typename PointT> int
+ writeBinary (const std::string &file_name,
+ const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices);
/** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format
* \param[in] cloud the point cloud data message
* \param[in] precision the specified output numeric stream precision (default: 8)
*/
- template <typename PointT> int
- writeASCII (const std::string &file_name,
+ template <typename PointT> int
+ writeASCII (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud,
const int precision = 8);
* \param[in] indices the set of point indices that we want written to disk
* \param[in] precision the specified output numeric stream precision (default: 8)
*/
- template <typename PointT> int
- writeASCII (const std::string &file_name,
+ template <typename PointT> int
+ writeASCII (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
const int precision = 8);
* future versions of PCL.
*/
template<typename PointT> inline int
- write (const std::string &file_name,
- const pcl::PointCloud<PointT> &cloud,
+ write (const std::string &file_name,
+ const pcl::PointCloud<PointT> &cloud,
const bool binary = false)
{
if (binary)
* future versions of PCL.
*/
template<typename PointT> inline int
- write (const std::string &file_name,
- const pcl::PointCloud<PointT> &cloud,
+ write (const std::string &file_name,
+ const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
bool binary = false)
{
namespace io
{
/** \brief Load a PCD v.6 file into a templated PointCloud type.
- *
+ *
* Any PCD files > v.6 will generate a warning as a
* pcl/PCLPointCloud2 message cannot hold the sensor origin.
*
* \param[out] cloud the resultant templated point cloud
* \ingroup io
*/
- inline int
+ inline int
loadPCDFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud)
{
pcl::PCDReader p;
* PCD_V7 - identity if not present)
* \ingroup io
*/
- inline int
+ inline int
loadPCDFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
Eigen::Vector4f &origin, Eigen::Quaternionf &orientation)
{
* future versions of PCL.
* \ingroup io
*/
- inline int
+ inline int
savePCDFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
- const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+ const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
const bool binary_mode = false)
{
return (w.write<PointT> (file_name, cloud, binary_mode));
}
- /**
+ /**
* \brief Templated version for saving point cloud data to a PCD file
* containing a specific given cloud format.
*
return (w.write<PointT> (file_name, cloud, false));
}
- /**
+ /**
* \brief Templated version for saving point cloud data to a PCD file
* containing a specific given cloud format. The resulting file will be an uncompressed binary.
*
return (w.write<PointT> (file_name, cloud, true));
}
- /**
+ /**
* \brief Templated version for saving point cloud data to a PCD file
* containing a specific given cloud format
*
* \ingroup io
*/
template<typename PointT> int
- savePCDFile (const std::string &file_name,
+ savePCDFile (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud,
- const std::vector<int> &indices,
+ const std::vector<int> &indices,
const bool binary_mode = false)
{
// Save the data
#pragma once
-#ifdef BUILD_Maintainer
-# if defined __GNUC__
-# pragma GCC system_header
-# elif defined _MSC_VER
-# pragma warning(push, 1)
-# endif
-#endif
-
#include <pcl/io/boost.h>
#include <pcl/io/ply/ply.h>
#include <pcl/io/ply/io_operators.h>
}
return (true);
}
-
-#ifdef BUILD_Maintainer
-# if defined __GNUC__
-# if __GNUC__ == 4 && __GNUC_MINOR__ > 3
-# pragma GCC diagnostic warning "-Weffc++"
-# pragma GCC diagnostic warning "-pedantic"
-# endif
-# elif defined _MSC_VER
-# pragma warning(pop)
-# endif
-#endif
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/io/boost.h>
#include <pcl/io/file_io.h>
+++ /dev/null
-#error "PXCGrabber was deprecated and removed, please use DepthSenseGrabber instead"
#pragma once
-#include <pcl/pcl_exports.h>
-
#include <pxcsession.h>
#include <pxccapture.h>
#include <pxccapturemanager.h>
-#include <boost/utility.hpp>
+#include <pcl/memory.h> // for pcl::shared_ptr, pcl::weak_ptr
+#include <pcl/pcl_exports.h> // for PCL_EXPORTS
+
+#include <boost/core/noncopyable.hpp> // for boost::noncopyable
-#include <memory>
+#include <cstddef> // for std::size_t
+#include <mutex> // for std::lock_guard, std::mutex
+#include <string> // for std::string
+#include <vector> // for std::vector
namespace pcl
{
pxcUID iuid;
pxcI32 didx;
std::string serial;
- std::weak_ptr<RealSenseDevice> device_ptr;
+ weak_ptr<RealSenseDevice> device_ptr;
inline bool isCaptured () { return (!device_ptr.expired ()); }
};
{
public:
-
- using Ptr = std::shared_ptr<RealSenseDevice>;
+ using Ptr = pcl::shared_ptr<RealSenseDevice>;
inline const std::string&
getSerialNumber () { return (device_id_); }
/** \brief template function to convert realsense point cloud to PCL point cloud
* \param[in] points - realsense point cloud array
- * \param[in] dynamic function to convert individual point color or intensity values
+ * \param[in] mapColorFunc dynamic function to convert individual point color or intensity values
*/
template <typename PointT, typename Functor>
typename pcl::PointCloud<PointT>::Ptr
#pragma once
#include <pcl/io/grabber.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/time.h>
#include <pcl/io/impl/synchronized_queue.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <boost/asio.hpp>
#include <memory>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
-pcl::EnsensoGrabber::grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud)
+pcl::EnsensoGrabber::grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud) const
{
if (!device_open_)
return (false);
pcl::EnsensoGrabber::setExtrinsicCalibration (const double euler_angle,
Eigen::Vector3d &rotation_axis,
const Eigen::Vector3d &translation,
- const std::string target)
+ const std::string target) const
{
if (!device_open_)
return (false);
pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point,
std::uint16_t azimuth,
HDLLaserReturn laserReturn,
- HDLLaserCorrection correction)
+ HDLLaserCorrection correction) const
{
double cos_azimuth, sin_azimuth;
}
hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp_listener_endpoint_);
}
- catch (const std::exception& bind)
+ catch (const std::exception&)
{
delete hdl_read_socket_;
hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp::endpoint (boost::asio::ip::address_v4::any (), udp_listener_endpoint_.port ()));
queue_consumer_thread_ = nullptr;
}
- if (hdl_read_socket_ != nullptr)
- {
- delete hdl_read_socket_;
- hdl_read_socket_ = nullptr;
- }
+ delete hdl_read_socket_;
+ hdl_read_socket_ = nullptr;
}
/////////////////////////////////////////////////////////////////////////////
#include <pcl/io/image_grabber.h>
#include <pcl/io/lzf_image_io.h>
#include <pcl/io/pcd_io.h>
+#include <pcl/memory.h>
#include <pcl/pcl_config.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
struct pcl::ImageGrabberBase::ImageGrabberImpl
{
//! Implementation of ImageGrabber
- ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
- const std::string& dir,
- float frames_per_second,
- bool repeat,
+ ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
+ const std::string& dir,
+ float frames_per_second,
+ bool repeat,
bool pclzf_mode=false);
//! For now, split rgb / depth folders only makes sense for VTK images
- ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
- const std::string& rgb_dir,
- const std::string& depth_dir,
- float frames_per_second,
+ ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
+ const std::string& rgb_dir,
+ const std::string& depth_dir,
+ float frames_per_second,
bool repeat);
- ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
- const std::vector<std::string>& depth_image_files,
- float frames_per_second,
+ ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
+ const std::vector<std::string>& depth_image_files,
+ float frames_per_second,
bool repeat);
-
- void
+
+ void
trigger ();
//! Read ahead -- figure out whether we are in VTK image or PCLZF mode
- void
+ void
loadNextCloud ();
-
+
//! Get cloud at a particular location
bool
getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
double &fx, double &fy, double &cx, double &cy) const;
-
+
//! Get cloud at a particular location
bool
getCloudVTK (std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const;
//! True if it is an image we know how to read
bool
- isValidExtension (const std::string &extension);
+ isValidExtension (const std::string &extension) const;
//! Convenience function to rewind to the last frame
void
std::size_t
numFrames () const;
-
+
#ifdef PCL_BUILT_WITH_VTK
//! Load an image file, return the vtkImageReader2, return false if it couldn't be opened
bool
getVtkImage (const std::string &filename, vtkSmartPointer<vtkImageData> &image) const;
#endif//PCL_BUILT_WITH_VTK
-
+
pcl::ImageGrabberBase& grabber_;
float frames_per_second_;
bool repeat_;
};
///////////////////////////////////////////////////////////////////////////////////////////
-pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
- const std::string& dir,
- float frames_per_second,
- bool repeat,
+pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
+ const std::string& dir,
+ float frames_per_second,
+ bool repeat,
bool pclzf_mode)
: grabber_ (grabber)
, frames_per_second_ (frames_per_second)
}
///////////////////////////////////////////////////////////////////////////////////////////
-pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
- const std::string& depth_dir,
- const std::string& rgb_dir,
- float frames_per_second,
+pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
+ const std::string& depth_dir,
+ const std::string& rgb_dir,
+ float frames_per_second,
bool repeat)
: grabber_ (grabber)
, frames_per_second_ (frames_per_second)
}
///////////////////////////////////////////////////////////////////////////////////////////
-pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
- const std::vector<std::string>& depth_image_files,
- float frames_per_second,
+pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber,
+ const std::vector<std::string>& depth_image_files,
+ float frames_per_second,
bool repeat)
: grabber_ (grabber)
, frames_per_second_ (frames_per_second)
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::ImageGrabberBase::ImageGrabberImpl::loadNextCloud ()
{
if (cur_frame_ >= numFrames ())
return;
}
}
- valid_ = getCloudAt (cur_frame_, next_cloud_, origin_, orientation_,
+ valid_ = getCloudAt (cur_frame_, next_cloud_, origin_, orientation_,
focal_length_x_, focal_length_y_, principal_point_x_, principal_point_y_);
cur_frame_++;
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::ImageGrabberBase::ImageGrabberImpl::trigger ()
{
if (valid_)
extension = boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ()));
pathname = itr->path ().string ();
basename = boost::filesystem::basename (itr->path ());
- if (!boost::filesystem::is_directory (itr->status ())
+ if (!boost::filesystem::is_directory (itr->status ())
&& isValidExtension (extension))
{
if (basename.find ("rgb") < std::string::npos)
extension = boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ()));
pathname = itr->path ().string ();
basename = boost::filesystem::basename (itr->path ());
- if (!boost::filesystem::is_directory (itr->status ())
+ if (!boost::filesystem::is_directory (itr->status ())
&& isValidExtension (extension))
{
if (basename.find ("rgb") < std::string::npos)
}
///////////////////////////////////////////////////////////////////////////////////////////
bool
-pcl::ImageGrabberBase::ImageGrabberImpl::isValidExtension (const std::string &extension)
+pcl::ImageGrabberBase::ImageGrabberImpl::isValidExtension (const std::string &extension) const
{
bool valid;
if(pclzf_mode_)
}
else
{
- valid = extension == ".TIFF" || extension == ".PNG"
+ valid = extension == ".TIFF" || extension == ".PNG"
|| extension == ".JPG" || extension == ".JPEG"
|| extension == ".PPM";
}
return (valid);
}
-
+
void
pcl::ImageGrabberBase::ImageGrabberImpl::rewindOnce ()
{
if (cur_frame_ > 0)
cur_frame_--;
}
-
+
//////////////////////////////////////////////////////////////////////////
bool
pcl::ImageGrabberBase::ImageGrabberImpl::getTimestampFromFilepath (
- const std::string &filepath,
+ const std::string &filepath,
std::uint64_t ×tamp) const
{
// For now, we assume the file is of the form frame_[22-char POSIX timestamp]_*
char timestamp_str[256];
- int result = std::sscanf (boost::filesystem::basename (filepath).c_str (),
+ int result = std::sscanf (boost::filesystem::basename (filepath).c_str (),
"frame_%22s_%*s",
timestamp_str);
if (result > 0)
}
return (false);
}
-
+
/////////////////////////////////////////////////////////////////////////////
bool
-pcl::ImageGrabberBase::ImageGrabberImpl::getCloudAt (std::size_t idx,
+pcl::ImageGrabberBase::ImageGrabberImpl::getCloudAt (std::size_t idx,
pcl::PCLPointCloud2 &blob,
- Eigen::Vector4f &origin,
- Eigen::Quaternionf &orientation,
- double &fx,
- double &fy,
- double &cx,
+ Eigen::Vector4f &origin,
+ Eigen::Quaternionf &orientation,
+ double &fx,
+ double &fy,
+ double &cx,
double &cy) const
{
if (!depth_image_files_.empty ())
}
bool
-pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx,
+pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx,
pcl::PCLPointCloud2 &blob,
- Eigen::Vector4f &origin,
+ Eigen::Vector4f &origin,
Eigen::Quaternionf &orientation) const
{
#ifdef PCL_BUILT_WITH_VTK
// Fill in image data
depth_pixel = static_cast<unsigned short*>(depth_image->GetScalarPointer ());
-
+
// Set up intrinsics
float scaleFactorX, scaleFactorY;
float centerX, centerY;
{
pcl::PointXYZRGBA &pt = cloud_color.at (x,y);
float depth = static_cast<float> (*depth_pixel) * depth_image_units_;
- if (depth == 0.0f)
+ if (depth == 0.0f)
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
else
{
pt.x = (static_cast<float> (x) - centerX) * scaleFactorX * depth;
- pt.y = (static_cast<float> (y) - centerY) * scaleFactorY * depth;
+ pt.y = (static_cast<float> (y) - centerY) * scaleFactorY * depth;
pt.z = depth;
}
{
pcl::PointXYZ &pt = cloud.at (x,y);
float depth = static_cast<float> (*depth_pixel) * depth_image_units_;
- if (depth == 0.0f)
+ if (depth == 0.0f)
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
else
{
pt.x = ((float)x - centerX) * scaleFactorX * depth;
- pt.y = ((float)y - centerY) * scaleFactorY * depth;
+ pt.y = ((float)y - centerY) * scaleFactorY * depth;
pt.z = depth;
}
}
}
bool
-pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (std::size_t idx,
+pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (std::size_t idx,
pcl::PCLPointCloud2 &blob,
- Eigen::Vector4f &origin,
- Eigen::Quaternionf &orientation,
- double &fx,
- double &fy,
- double &cx,
+ Eigen::Vector4f &origin,
+ Eigen::Quaternionf &orientation,
+ double &fx,
+ double &fy,
+ double &cx,
double &cy) const
{
if (idx > depth_pclzf_files_.size ())
fy = focal_length_y_;
cx = principal_point_x_;
cy = principal_point_y_;
- rgb.setParameters (manual_params);
- yuv.setParameters (manual_params);
- bayer.setParameters (manual_params);
- depth.setParameters (manual_params);
+ rgb.setParameters (manual_params);
+ yuv.setParameters (manual_params);
+ bayer.setParameters (manual_params);
+ depth.setParameters (manual_params);
}
else
{
fy = focal_length_y_;
cx = principal_point_x_;
cy = principal_point_y_;
- depth.setParameters (manual_params);
+ depth.setParameters (manual_params);
}
else
{
origin = Eigen::Vector4f::Zero ();
orientation = Eigen::Quaternionf::Identity ();
return (true);
-}
-
+}
+
////////////////////////////////////////////////////////////////////////
//
#ifdef PCL_BUILT_WITH_VTK
bool
pcl::ImageGrabberBase::ImageGrabberImpl::getVtkImage (
- const std::string &filename,
+ const std::string &filename,
vtkSmartPointer<vtkImageData> &image) const
{
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::ImageGrabberBase::start ()
{
if (impl_->frames_per_second_ > 0)
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::ImageGrabberBase::stop ()
{
if (impl_->frames_per_second_ > 0)
}
///////////////////////////////////////////////////////////////////////////////////////////
-bool
+bool
pcl::ImageGrabberBase::isRunning () const
{
return (impl_->running_);
}
///////////////////////////////////////////////////////////////////////////////////////////
-std::string
+std::string
pcl::ImageGrabberBase::getName () const
{
return ("ImageGrabber");
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::ImageGrabberBase::rewind ()
{
impl_->cur_frame_ = 0;
}
///////////////////////////////////////////////////////////////////////////////////////////
-float
+float
pcl::ImageGrabberBase::getFramesPerSecond () const
{
return (impl_->frames_per_second_);
}
///////////////////////////////////////////////////////////////////////////////////////////
-bool
+bool
pcl::ImageGrabberBase::isRepeatOn () const
{
return (impl_->repeat_);
///////////////////////////////////////////////////////////////////////////////////////////
void
-pcl::ImageGrabberBase::setRGBImageFiles (const std::vector<std::string>& rgb_image_files)
+pcl::ImageGrabberBase::setRGBImageFiles (const std::vector<std::string>& rgb_image_files)
{
impl_->rgb_image_files_ = rgb_image_files;
impl_->cur_frame_ = 0;
///////////////////////////////////////////////////////
void
-pcl::ImageGrabberBase::setCameraIntrinsics (const double focal_length_x,
- const double focal_length_y,
- const double principal_point_x,
+pcl::ImageGrabberBase::setCameraIntrinsics (const double focal_length_x,
+ const double focal_length_y,
+ const double principal_point_x,
const double principal_point_y)
{
impl_->focal_length_x_ = focal_length_x;
}
void
-pcl::ImageGrabberBase::getCameraIntrinsics (double &focal_length_x,
- double &focal_length_y,
- double &principal_point_x,
+pcl::ImageGrabberBase::getCameraIntrinsics (double &focal_length_x,
+ double &focal_length_y,
+ double &principal_point_x,
double &principal_point_y) const
{
focal_length_x = impl_->focal_length_x_;
bool
pcl::ImageGrabberBase::getCloudAt (std::size_t idx,
pcl::PCLPointCloud2 &blob,
- Eigen::Vector4f &origin,
+ Eigen::Vector4f &origin,
Eigen::Quaternionf &orientation) const
{
double fx, fy, cx, cy;
std::string basename = boost::filesystem::basename (pathname);
return (basename);
}
-
+
/////////////////////////////////////////////////////////////////////////////////////////
std::string
pcl::ImageGrabberBase::getDepthFileNameAtIndex (std::size_t idx) const
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2000-2010 Marc Alexander Lehmann <schmorp@schmorp.de>
* Copyright (c) 2010-2011, Willow Garage, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*
*/
-#include <pcl/io/lzf.h>
-#include <cstring>
-#include <climits>
#include <pcl/console/print.h>
+#include <pcl/io/lzf.h>
+
#include <cerrno>
+#include <climits>
+#include <cstddef>
+#include <cstring>
/*
* Size of hashtable is (1 << HLOG) * sizeof (char *)
hval = (hval << 8) | ip[2];
hslot = htab + IDX (hval);
- const unsigned char *ref = *hslot + (static_cast<const unsigned char*> (in_data));
+ const unsigned char *ref = *hslot + (static_cast<const unsigned char*> (in_data));
*hslot = static_cast<unsigned int> (ip - (static_cast<const unsigned char*> (in_data)));
// off requires a type wide enough to hold a general pointer difference.
// special workaround for it.
#if defined (WIN32) && defined (_M_X64) && defined (_MSC_VER)
// workaround for missing POSIX compliance
- unsigned _int64 off;
+ unsigned _int64 off;
#else
unsigned long off;
#endif
if (
// The next test will actually take care of this, but this is faster if htab is initialized
- ref < ip
+ ref < ip
&& (off = ip - ref - 1) < (1 << 13)
&& ref > static_cast<const unsigned char *> (in_data)
&& ref[2] == ip[2]
{
// Match found at *ref++
unsigned int len = 2;
- ptrdiff_t maxlen = in_end - ip - len;
+ std::ptrdiff_t maxlen = in_end - ip - len;
maxlen = maxlen > ((1 << 8) + (1 << 3)) ? ((1 << 8) + (1 << 3)) : maxlen;
// First a faster conservative test
}
}
- // At most 3 bytes can be missing here
+ // At most 3 bytes can be missing here
if (op + 3 > out_end)
return (0);
}
///////////////////////////////////////////////////////////////////////////////////////////
-unsigned int
+unsigned int
pcl::lzfDecompress (const void *const in_data, unsigned int in_len,
void *out_data, unsigned int out_len)
{
{
boost::property_tree::xml_parser::read_xml (filename, pt, boost::property_tree::xml_parser::trim_whitespace);
}
- catch (std::exception& e)
+ catch (std::exception&)
{}
pt.put (tag, parameter);
{
boost::property_tree::xml_parser::read_xml (filename, pt, boost::property_tree::xml_parser::trim_whitespace);
}
- catch (std::exception& e)
+ catch (std::exception&)
{}
pt.put ("depth.focal_length_x", parameters.focal_length_x);
{
boost::property_tree::xml_parser::read_xml (filename, pt, boost::property_tree::xml_parser::trim_whitespace);
}
- catch (std::exception& e)
+ catch (std::exception&)
{}
pt.put ("rgb.focal_length_x", parameters.focal_length_x);
}
++point_idx;
}
- catch (const boost::bad_lexical_cast &e)
+ catch (const boost::bad_lexical_cast&)
{
PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
return (-1);
}
++normal_idx;
}
- catch (const boost::bad_lexical_cast &e)
+ catch (const boost::bad_lexical_cast&)
{
PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
return (-1);
}
++v_idx;
}
- catch (const boost::bad_lexical_cast &e)
+ catch (const boost::bad_lexical_cast&)
{
PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
return (-1);
}
++vn_idx;
}
- catch (const boost::bad_lexical_cast &e)
+ catch (const boost::bad_lexical_cast&)
{
PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
return (-1);
coordinates.emplace_back(c[0]/c[2], c[1]/c[2]);
++vt_idx;
}
- catch (const boost::bad_lexical_cast &e)
+ catch (const boost::bad_lexical_cast&)
{
PCL_ERROR ("Unable to convert line %s to texture coordinates!", line.c_str ());
return (-1);
}
++v_idx;
}
- catch (const boost::bad_lexical_cast &e)
+ catch (const boost::bad_lexical_cast&)
{
PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
return (-1);
}
++vn_idx;
}
- catch (const boost::bad_lexical_cast &e)
+ catch (const boost::bad_lexical_cast&)
{
PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
return (-1);
#include <pcl/point_types.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
-#include <pcl/io/boost.h>
+#include <pcl/io/boost.h> // for boost::shared_array
+#include <pcl/memory.h> // for dynamic_pointer_cast
#include <pcl/exceptions.h>
+
#include <iostream>
namespace
, point_cloud_rgba_signal_ ()
{
openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
- device_ = boost::dynamic_pointer_cast< openni_wrapper::DeviceONI> (driver.createVirtualDevice (file_name, repeat, stream));
+ device_ = dynamic_pointer_cast< openni_wrapper::DeviceONI> (driver.createVirtualDevice (file_name, repeat, stream));
if (!device_->hasDepthStream ())
PCL_THROW_EXCEPTION (pcl::IOException, "Device does not provide 3D information.");
* Author: Julius Kammerl (jkammerl@willowgarage.com)
*/
-#include "pcl/io/openni2/openni2_convert.h"
-#include "pcl/io/io_exception.h"
-
-#include <boost/make_shared.hpp>
+#include <pcl/memory.h>
+#include <pcl/io/io_exception.h>
+#include <pcl/io/openni2/openni2_convert.h>
#include <string>
+
namespace pcl
{
namespace io
#include "pcl/io/openni2/openni2_convert.h"
#include "pcl/io/openni2/openni2_device.h"
#include "pcl/io/io_exception.h"
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <mutex>
#include <set>
}
OpenNI2Device::Ptr
-pcl::io::openni2::OpenNI2DeviceManager::getDeviceByIndex (int index)
+pcl::io::openni2::OpenNI2DeviceManager::getDeviceByIndex (int index) const
{
auto URIs = getConnectedDeviceURIs ();
return pcl::make_shared<OpenNI2Device>( URIs->at (index) );
*
*/
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#ifdef __GNUC__
*
*/
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#ifdef __GNUC__
*
*/
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#ifdef __GNUC__
*
*/
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#ifdef __GNUC__
*
*/
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#ifdef __GNUC__
*
*/
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include <pcl/io/openni_camera/openni_image_bayer_grbg.h>
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include <pcl/io/openni_camera/openni_image_rgb24.h>
*
*/
#include <pcl/pcl_config.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include <pcl/io/openni_camera/openni_image_yuv_422.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/tar.h>
+#include <pcl/memory.h>
#include <pcl/pcl_config.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
PCDGrabberImpl (pcl::PCDGrabberBase& grabber, const std::vector<std::string>& pcd_files, float frames_per_second, bool repeat);
void trigger ();
void readAhead ();
-
+
// TAR reading I/O
int openTARFile (const std::string &file_name);
void closeTARFile ();
bool readTARHeader ();
-
+
//! Initialize (find the locations of all clouds, if we haven't yet)
void
scrapeForClouds (bool force=false);
//! Get cloud at a particular location
bool
- getCloudAt (std::size_t idx,
+ getCloudAt (std::size_t idx,
pcl::PCLPointCloud2 &blob,
- Eigen::Vector4f &origin,
+ Eigen::Vector4f &origin,
Eigen::Quaternionf &orientation);
//! Returns the size
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::PCDGrabberBase::PCDGrabberImpl::readAhead ()
{
PCDReader reader;
return (false);
}
- // We only support regular files for now.
- // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices,
+ // We only support regular files for now.
+ // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices,
// directories, and named pipes.
if (tar_header_.file_type[0] != '0' && tar_header_.file_type[0] != '\0')
{
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::PCDGrabberBase::PCDGrabberImpl::trigger ()
{
std::lock_guard<std::mutex> read_ahead_lock(read_ahead_mutex_);
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::PCDGrabberBase::PCDGrabberImpl::scrapeForClouds (bool force)
{
// Do nothing if we've already scraped (unless force is set)
}
///////////////////////////////////////////////////////////////////////////////////////////
-bool
-pcl::PCDGrabberBase::PCDGrabberImpl::getCloudAt (std::size_t idx,
+bool
+pcl::PCDGrabberBase::PCDGrabberImpl::getCloudAt (std::size_t idx,
pcl::PCLPointCloud2 &blob,
- Eigen::Vector4f &origin,
+ Eigen::Vector4f &origin,
Eigen::Quaternionf &orientation)
{
scrapeForClouds (); // Make sure we've scraped
if (idx >= numFrames ())
return false;
-
+
PCDReader reader;
int pcd_version;
std::string filename = pcd_files_[cloud_idx_to_file_idx_[idx]];
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::PCDGrabberBase::start ()
{
if (impl_->frames_per_second_ > 0)
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::PCDGrabberBase::stop ()
{
if (impl_->frames_per_second_ > 0)
}
///////////////////////////////////////////////////////////////////////////////////////////
-bool
+bool
pcl::PCDGrabberBase::isRunning () const
{
return (impl_->running_ && (impl_->pcd_iterator_ != impl_->pcd_files_.end()));
}
///////////////////////////////////////////////////////////////////////////////////////////
-std::string
+std::string
pcl::PCDGrabberBase::getName () const
{
return ("PCDGrabber");
}
///////////////////////////////////////////////////////////////////////////////////////////
-void
+void
pcl::PCDGrabberBase::rewind ()
{
impl_->pcd_iterator_ = impl_->pcd_files_.begin ();
}
///////////////////////////////////////////////////////////////////////////////////////////
-float
+float
pcl::PCDGrabberBase::getFramesPerSecond () const
{
return (impl_->frames_per_second_);
}
///////////////////////////////////////////////////////////////////////////////////////////
-bool
+bool
pcl::PCDGrabberBase::isRepeatOn () const
{
return (impl_->repeat_);
///////////////////////////////////////////////////////////////////////////////////////////
bool
-pcl::PCDGrabberBase::getCloudAt (std::size_t idx,
+pcl::PCDGrabberBase::getCloudAt (std::size_t idx,
pcl::PCLPointCloud2 &blob,
- Eigen::Vector4f &origin,
+ Eigen::Vector4f &origin,
Eigen::Quaternionf &orientation) const
{
return (impl_->getCloudAt (idx, blob, origin, orientation));
{
return (impl_->numFrames ());
}
-
const auto cloud_vertices_ptr = points.get_vertices ();
const auto cloud_texture_ptr = points.get_texture_coordinates ();
-#ifdef _OPENMP
-#pragma omp parallel for
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(cloud, cloud_vertices_ptr, mapColorFunc)
for (int index = 0; index < cloud->points.size (); ++index)
{
const auto ptr = cloud_vertices_ptr + index;
*/
#include <pcl/io/robot_eye_grabber.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/console/print.h>
/////////////////////////////////////////////////////////////////////////////
#include <pcl/io/auto_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/io/vtk_lib_io.h>
+#include <pcl/memory.h> // for pcl::make_shared
-#include <boost/make_shared.hpp>
+#include <boost/filesystem.hpp> // for boost::filesystem::path
+#include <boost/algorithm/string.hpp> // for boost::algorithm::ends_with
#define ASCII 0
#define BINARY 1
{
if (!input.polygons.empty ())
PCL_WARN ("PCD file format does not support meshes! Only points be saved.\n");
- pcl::PCLPointCloud2::Ptr cloud =
boost::make_shared<pcl::PCLPointCloud2> (input.cloud);
+ pcl::PCLPointCloud2::Ptr cloud =
pcl::make_shared<pcl::PCLPointCloud2> (input.cloud);
if (!savePointCloud (cloud, output_file, output_type))
return (false);
}
SimpleOpenNIProcessor (openni_wrapper::OpenNIDevice::DepthMode depth_mode = openni_wrapper::OpenNIDevice::OpenNI_12_bit_depth) : mode (depth_mode) {}
void
- cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
+ cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud) const
{
static unsigned count = 0;
static double last = pcl::getTime ();
#include <pcl/pcl_macros.h>
-PCL_PRAGMA_WARNING("This header is deprecated and will be removed in an upcoming release.")
+PCL_DEPRECATED_HEADER(1, 12, "")
#if defined _MSC_VER
# pragma warning(disable: 4267 4244)
#pragma once
#include <climits>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
*/
void
detectKeypoints (const std::vector<unsigned char> &intensity_data,
- pcl::PointCloud<pcl::PointUV> &output);
+ pcl::PointCloud<pcl::PointUV> &output) const;
/** \brief Detects corner points.
* \param intensity_data
*/
void
detectKeypoints (const std::vector<float> &intensity_data,
- pcl::PointCloud<pcl::PointUV> &output);
+ pcl::PointCloud<pcl::PointUV> &output) const;
/** \brief Applies non-max-suppression.
* \param[in] intensity_data the image data
void
computeCornerScores (const unsigned char* im,
const std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > & corners_all,
- std::vector<ScoreIndex> & scores);
+ std::vector<ScoreIndex> & scores) const;
/** \brief Computes corner scores for the specified points.
* \param im
void
computeCornerScores (const float* im,
const std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > & corners_all,
- std::vector<ScoreIndex> & scores);
+ std::vector<ScoreIndex> & scores) const;
/** \brief Width of the image to process. */
std::size_t width_;
#pragma once
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/keypoints/agast_2d.h>
+
namespace pcl
{
/** \brief Detects BRISK interest points based on the original code and paper
#include <pcl/common/io.h>
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointInT, typename PointOutT, typename IntensityT> bool
-
pcl::AgastKeypoint2DBase<PointInT, PointOutT, IntensityT>::initCompute ()
+AgastKeypoint2DBase<PointInT, PointOutT, IntensityT>::initCompute ()
{
if (!pcl::Keypoint<PointInT, PointOutT>::initCompute ())
{
}
if (!input_->isOrganized ())
- {
+ {
PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
return (false);
}
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT> void
-
pcl::AgastKeypoint2D<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output)
+AgastKeypoint2D<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output)
{
// image size
const std::size_t width = input_->width;
output.is_dense = true;
}
+} // namespace pcl
#define AgastKeypoint2D(T,I) template class PCL_EXPORTS pcl::AgastKeypoint2D<T,I>;
-#endif
+
+#endif
+
#include <pcl/common/io.h>
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointInT, typename PointOutT, typename IntensityT> bool
-
pcl::BriskKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute ()
+BriskKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute ()
{
if (!pcl::Keypoint<PointInT, PointOutT>::initCompute ())
{
}
if (!input_->isOrganized ())
- {
+ {
PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
return (false);
}
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::BriskKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
+BriskKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
// image size
const int width = int (input_->width);
}
}
-#endif
+} // namespace pcl
+
+#endif
+
*
*/
+
#ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
#define PCL_HARRIS_KEYPOINT_2D_IMPL_H_
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setMethod (ResponseMethod method)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setMethod (ResponseMethod method)
{
method_ = method;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setThreshold (float threshold)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setThreshold (float threshold)
{
threshold_= threshold;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setRefine (bool do_refine)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setRefine (bool do_refine)
{
refine_ = do_refine;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setNonMaxSupression (bool nonmax)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setNonMaxSupression (bool nonmax)
{
nonmax_ = nonmax;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setWindowWidth (int window_width)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setWindowWidth (int window_width)
{
window_width_= window_width;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setWindowHeight (int window_height)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setWindowHeight (int window_height)
{
window_height_= window_height;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setSkippedPixels (int skipped_pixels)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setSkippedPixels (int skipped_pixels)
{
skipped_pixels_= skipped_pixels;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setMinimalDistance (int min_distance)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setMinimalDistance (int min_distance)
{
min_distance_ = min_distance;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::computeSecondMomentMatrix (std::size_t index, float* coefficients) const
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::computeSecondMomentMatrix (std::size_t index, float* coefficients) const
{
static const int width = static_cast<int> (input_->width);
static const int height = static_cast<int> (input_->height);
-
+
int x = static_cast<int> (index % input_->width);
int y = static_cast<int> (index / input_->width);
// indices 0 1 2
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> bool
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute ()
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute ()
{
if (!pcl::Keypoint<PointInT, PointOutT>::initCompute ())
{
}
if (!input_->isOrganized ())
- {
+ {
PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
return (false);
}
-
+
if (indices_->size () != input_->size ())
{
PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
return (false);
}
-
+
if ((window_height_%2) == 0)
{
PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
return (false);
}
-
+
half_window_width_ = window_width_ / 2;
half_window_height_ = window_height_ / 2;
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
derivatives_cols_.resize (input_->width, input_->height);
derivatives_rows_.resize (input_->width, input_->height);
//Compute cloud intensities first derivatives along columns and rows
- //!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan
+ //!!! nsallem 20120220 : we don't test here for density so if one term is nan the result is nan
int w = static_cast<int> (input_->width) - 1;
int h = static_cast<int> (input_->height) - 1;
// j = 0 --> j-1 out of range ; use 0
derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
for(int i = 1; i < w; ++i)
- {
+ {
derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
- }
+ }
derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
responseTomasi(*response_);
break;
}
-
+
if (!nonmax_)
{
output = *response_;
keypoints_indices_->indices.push_back (i);
}
else
- {
+ {
std::sort (indices_->begin (), indices_->end (), [this] (int p1, int p2) { return greaterIntensityAtIndices (p1, p2); });
- float threshold = threshold_ * response_->points[indices_->front ()].intensity;
+ const float threshold = threshold_ * response_->points[indices_->front ()].intensity;
output.clear ();
output.reserve (response_->size());
- std::vector<bool> occupency_map (response_->size (), false);
+ std::vector<bool> occupency_map (response_->size (), false);
int width (response_->width);
int height (response_->height);
const int occupency_map_size (occupency_map.size ());
-#ifdef _OPENMP
-#pragma omp parallel for shared (output, occupency_map) firstprivate (width, height) num_threads(threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(occupency_map, output) \
+ firstprivate(width, height) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(occupency_map, occupency_map_size, output, threshold) \
+ firstprivate(width, height) \
+ num_threads(threads_)
#endif
for (int i = 0; i < occupency_map_size; ++i)
{
const PointOutT& point_out = response_->points [idx];
if (occupency_map[idx] || point_out.intensity < threshold || !isFinite (point_out))
continue;
-
-#ifdef _OPENMP
+
#pragma omp critical
-#endif
{
output.push_back (point_out);
keypoints_indices_->indices.push_back (idx);
}
-
- int u_end = std::min (width, idx % width + min_distance_);
- int v_end = std::min (height, idx / width + min_distance_);
+
+ int u_end = std::min (width, idx % width + min_distance_);
+ int v_end = std::min (height, idx / width + min_distance_);
for(int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
for(int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
occupency_map[v*input_->width+u] = true;
output.is_dense = input_->is_dense;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseHarris (PointCloudOut &output) const
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseHarris (PointCloudOut &output) const
{
PCL_ALIGN (16) float covar [3];
output.clear ();
output.resize (input_->size ());
const int output_size (output.size ());
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (covar) num_threads(threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(covar) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(output, output_size) \
+ private(covar) \
+ num_threads(threads_)
#endif
for (int index = 0; index < output_size; ++index)
{
output.width = input_->width;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseNoble (PointCloudOut &output) const
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseNoble (PointCloudOut &output) const
{
PCL_ALIGN (16) float covar [3];
output.clear ();
output.resize (input_->size ());
const int output_size (output.size ());
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (covar) num_threads(threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(covar) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(output, output_size) \
+ private(covar) \
+ num_threads(threads_)
#endif
for (int index = 0; index < output_size; ++index)
{
out_point.z = in_point.z;
out_point.intensity = 0;
if (isFinite (in_point))
- {
+ {
computeSecondMomentMatrix (index, covar);
float trace = covar [0] + covar [2];
if (trace != 0)
float det = covar[0] * covar[2] - covar[1] * covar[1];
out_point.intensity = det / trace;
}
- }
+ }
}
-
+
output.height = input_->height;
output.width = input_->width;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseLowe (PointCloudOut &output) const
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseLowe (PointCloudOut &output) const
{
PCL_ALIGN (16) float covar [3];
output.clear ();
output.resize (input_->size ());
const int output_size (output.size ());
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (covar) num_threads(threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(covar) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(output, output_size) \
+ private(covar) \
+ num_threads(threads_)
#endif
- for (int index = 0; index < output_size; ++index)
+ for (int index = 0; index < output_size; ++index)
{
PointOutT &out_point = output.points [index];
const PointInT &in_point = input_->points [index];
out_point.z = in_point.z;
out_point.intensity = 0;
if (isFinite (in_point))
- {
+ {
computeSecondMomentMatrix (index, covar);
float trace = covar [0] + covar [2];
if (trace != 0)
output.width = input_->width;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseTomasi (PointCloudOut &output) const
+HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseTomasi (PointCloudOut &output) const
{
PCL_ALIGN (16) float covar [3];
output.clear ();
output.resize (input_->size ());
const int output_size (output.size ());
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (covar) num_threads(threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(covar) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(output, output_size) \
+ private(covar) \
+ num_threads(threads_)
#endif
for (int index = 0; index < output_size; ++index)
{
out_point.z = in_point.z;
out_point.intensity = 0;
if (isFinite (in_point))
- {
+ {
computeSecondMomentMatrix (index, covar);
// min egenvalue
out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
- }
+ }
}
-
+
output.height = input_->height;
output.width = input_->width;
}
-// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-// template <typename PointInT, typename PointOutT, typename IntensityT> void
-// pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::refineCorners (PointCloudOut &corners) const
-// {
-// std::vector<int> nn_indices;
-// std::vector<float> nn_dists;
-
-// Eigen::Matrix2f nnT;
-// Eigen::Matrix2f NNT;
-// Eigen::Matrix2f NNTInv;
-// Eigen::Vector2f NNTp;
-// float diff;
-// const unsigned max_iterations = 10;
-// #ifdef _OPENMP
-// #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff, nn_indices, nn_dists) num_threads(threads_)
-// #endif
-// for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
-// {
-// unsigned iterations = 0;
-// do {
-// NNT.setZero();
-// NNTp.setZero();
-// PointInT corner;
-// corner.x = corners[cIdx].x;
-// corner.y = corners[cIdx].y;
-// corner.z = corners[cIdx].z;
-// tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
-// for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
-// {
-// if (!std::isfinite (normals_->points[*iIt].normal_x))
-// continue;
-
-// nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
-// NNT += nnT;
-// NNTp += nnT * surface_->points[*iIt].getVector3fMap ();
-// }
-// if (invert3x3SymMatrix (NNT, NNTInv) != 0)
-// corners[cIdx].getVector3fMap () = NNTInv * NNTp;
-
-// diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
-// } while (diff > 1e-6 && ++iterations < max_iterations);
-// }
-// }
+} // namespace pcl
#define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
+
#endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
+
output.points.clear ();
output.points.reserve (response->points.size());
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output, response) \
+ num_threads(threads_)
for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
{
if (!isFinite (response->points[idx]) ||
}
}
if (is_maxima)
-#ifdef _OPENMP
#pragma omp critical
-#endif
{
output.points.push_back (response->points[idx]);
keypoints_indices_->indices.push_back (idx);
{
PCL_ALIGN (16) float covar [8];
output.resize (input_->size ());
-#ifdef _OPENMP
- #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(covar) \
+ num_threads(threads_)
for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
{
const PointInT& pointIn = input_->points [pIdx];
{
PCL_ALIGN (16) float covar [8];
output.resize (input_->size ());
-#ifdef _OPENMP
- #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
-#endif
+#pragma omp parallel \
+ for default(none) \
+ shared(output) \
+ private(covar) \
+ num_threads(threads_)
for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
{
const PointInT& pointIn = input_->points [pIdx];
{
PCL_ALIGN (16) float covar [8];
output.resize (input_->size ());
-#ifdef _OPENMP
- #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(covar) \
+ num_threads(threads_)
for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
{
const PointInT& pointIn = input_->points [pIdx];
PCL_ALIGN (16) float covar [8];
Eigen::Matrix3f covariance_matrix;
output.resize (input_->size ());
-#ifdef _OPENMP
- #pragma omp parallel for shared (output) private (covar, covariance_matrix) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output) \
+ private(covar, covariance_matrix) \
+ num_threads(threads_)
for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
{
const PointInT& pointIn = input_->points [pIdx];
Eigen::Vector3f NNTp;
float diff;
const unsigned max_iterations = 10;
-#ifdef _OPENMP
- #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(corners) \
+ private(nnT, NNT, NNTInv, NNTp, diff) \
+ num_threads(threads_)
for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
{
unsigned iterations = 0;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
cloud->resize (surface_->size ());
-#ifdef _OPENMP
- #pragma omp parallel for num_threads(threads_) default(shared)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (unsigned idx = 0; idx < surface_->size (); ++idx)
{
cloud->points [idx].x = surface_->points [idx].x;
grad_est.setRadiusSearch (search_radius_);
grad_est.compute (*intensity_gradients_);
-#ifdef _OPENMP
- #pragma omp parallel for num_threads(threads_) default (shared)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (std::size_t idx = 0; idx < intensity_gradients_->size (); ++idx)
{
float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x +
output.points.clear ();
output.points.reserve (response->points.size());
-#ifdef _OPENMP
- #pragma omp parallel for num_threads(threads_) default(shared)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (std::size_t idx = 0; idx < response->points.size (); ++idx)
{
if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_)
}
}
if (is_maxima)
-#ifdef _OPENMP
#pragma omp critical
-#endif
{
output.points.push_back (response->points[idx]);
keypoints_indices_->indices.push_back (idx);
Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
Eigen::Matrix<float, 6, 6> covariance;
-#ifdef _OPENMP
- #pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ private(pointOut, covar, covariance, solver) \
+ num_threads(threads_)
for (unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
{
const PointInT& pointIn = input_->points [pIdx];
pointOut.x = pointIn.x;
pointOut.y = pointIn.y;
pointOut.z = pointIn.z;
-#ifdef _OPENMP
- #pragma omp critical
-#endif
+ #pragma omp critical
output.points.push_back(pointOut);
}
output.height = input_->height;
pcl::BoundaryEstimation<PointInT, NormalT, pcl::Boundary> boundary_estimator;
boundary_estimator.setInputCloud (input_);
-#ifdef _OPENMP
-#pragma omp parallel for private(u, v) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(angle_threshold, boundary_estimator, border_radius, edge_points, input) \
+ private(u, v) \
+ num_threads(threads_)
for (int index = 0; index < int (input.points.size ()); index++)
{
edge_points[index] = false;
return (false);
}
- if (third_eigen_value_)
delete[] third_eigen_value_;
third_eigen_value_ = new double[input_->size ()];
memset(third_eigen_value_, 0, sizeof(double) * input_->size ());
- if (edge_points_)
delete[] edge_points_;
if (border_radius_ > 0.0)
bool* borders = new bool [input_->size()];
-#ifdef _OPENMP
- #pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(borders) \
+ num_threads(threads_)
for (int index = 0; index < int (input_->size ()); index++)
{
borders[index] = false;
for (std::size_t i = 0; i < input_->size (); i++)
prg_mem[i] = prg_local_mem + 3 * i;
-#ifdef _OPENMP
- #pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(borders, omp_mem, prg_mem) \
+ num_threads(threads_)
for (int index = 0; index < static_cast<int> (input_->size ()); index++)
{
#ifdef _OPENMP
}
bool* feat_max = new bool [input_->size()];
- bool is_max;
-#ifdef _OPENMP
- #pragma omp parallel for private(is_max) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(feat_max) \
+ num_threads(threads_)
for (int index = 0; index < int (input_->size ()); index++)
{
feat_max [index] = false;
if (n_neighbors >= min_neighbors_)
{
- is_max = true;
+ bool is_max = true;
for (int j = 0 ; j < n_neighbors; j++)
if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
}
}
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(feat_max, output) \
+ num_threads(threads_)
for (int index = 0; index < int (input_->size ()); index++)
{
if (feat_max[index])
-#ifdef _OPENMP
#pragma omp critical
-#endif
{
PointOutT p;
p.getVector3fMap () = input_->points[index].getVector3fMap ();
*
*/
+
#ifndef PCL_KEYPOINT_IMPL_H_
#define PCL_KEYPOINT_IMPL_H_
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointInT, typename PointOutT> bool
-
pcl::Keypoint<PointInT, PointOutT>::initCompute ()
+Keypoint<PointInT, PointOutT>::initCompute ()
{
if (!PCLBase<PointInT>::initCompute ())
return (false);
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT> inline void
-
pcl::Keypoint<PointInT, PointOutT>::compute (PointCloudOut &output)
+Keypoint<PointInT, PointOutT>::compute (PointCloudOut &output)
{
if (!initCompute ())
{
surface_.reset ();
}
+} // namespace pcl
+
#endif //#ifndef PCL_KEYPOINT_IMPL_H_
label_idx_ = pcl::getFieldIndex<PointOutT> ("label", out_fields_);
const int input_size = static_cast<int> (input_->size ());
-//#ifdef _OPENMP
-//#pragma omp parallel for shared (response) num_threads(threads_)
-//#endif
for (int point_index = 0; point_index < input_size; ++point_index)
{
const PointInT& point_in = input_->points [point_index];
memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
&label, sizeof (std::uint32_t));
}
-//#ifdef _OPENMP
-//#pragma omp critical
-//#endif
response->push_back (point_out);
}
else
memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
&label, sizeof (std::uint32_t));
}
-//#ifdef _OPENMP
-//#pragma omp critical
-//#endif
response->push_back (point_out);
}
}
output.points.clear ();
output.points.reserve (response->points.size());
-//#ifdef _OPENMP
-//#pragma omp parallel for shared (output) num_threads(threads_)
-//#endif
for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
{
const PointOutT& point_in = response->points [idx];
}
}
if (is_minima)
-//#ifdef _OPENMP
-//#pragma omp critical
-//#endif
{
output.points.push_back (response->points[idx]);
keypoints_indices_->indices.push_back (idx);
*
*/
+
#ifndef PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_
#define PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_
+
+namespace pcl
+{
+
template <typename PointInT, typename PointOutT, typename IntensityT> bool
-
pcl::TrajkovicKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute ()
+TrajkovicKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute ()
{
if (!PCLBase<PointInT>::initCompute ())
return (false);
return (true);
}
-/////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename IntensityT> void
-
pcl::TrajkovicKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
+TrajkovicKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
- int w = static_cast<int> (input_->width) - half_window_size_;
- int h = static_cast<int> (input_->height) - half_window_size_;
+ const int w = static_cast<int> (input_->width) - half_window_size_;
+ const int h = static_cast<int> (input_->height) - half_window_size_;
if (method_ == pcl::TrajkovicKeypoint2D<PointInT, PointOutT, IntensityT>::FOUR_CORNERS)
{
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(h, w) \
+ num_threads(threads_)
#endif
for(int j = half_window_size_; j < h; ++j)
{
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(h, w) \
+ num_threads(threads_)
#endif
for(int j = half_window_size_; j < h; ++j)
{
const int width (input_->width);
const int height (input_->height);
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(indices, occupency_map, output) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(height, indices, occupency_map, output, width) \
+ num_threads(threads_)
#endif
for (std::size_t i = 0; i < indices.size (); ++i)
{
p.getVector3fMap () = input_->points[idx].getVector3fMap ();
p.intensity = response_->points [idx];
-#ifdef _OPENMP
#pragma omp critical
-#endif
{
output.push_back (p);
keypoints_indices_->indices.push_back (idx);
output.is_dense = input_->is_dense;
}
+} // namespace pcl
+
#define PCL_INSTANTIATE_TrajkovicKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::TrajkovicKeypoint2D<T,U,I>;
+
#endif
+
#include <pcl/features/integral_image_normal.h>
+
+namespace pcl
+{
+
template <typename PointInT, typename PointOutT, typename NormalT> bool
-
pcl::TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::initCompute ()
+TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::initCompute ()
{
if (!PCLBase<PointInT>::initCompute ())
return (false);
return (true);
}
-/////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename PointOutT, typename NormalT> void
-
pcl::TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
+TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
const Normals &normals = *normals_;
if (method_ == FOUR_CORNERS)
{
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(input, normals, response) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(h, input, normals, response, w) \
+ num_threads(threads_)
#endif
for(int j = half_window_size_; j < h; ++j)
{
}
else
{
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(input, normals, response) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(h, input, normals, response, w) \
+ num_threads(threads_)
#endif
for(int j = half_window_size_; j < h; ++j)
{
const int width (input_->width);
const int height (input_->height);
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
+#pragma omp parallel for \
+ default(none) \
+ shared(indices, occupency_map, output) \
+ num_threads(threads_)
+#else
+#pragma omp parallel for \
+ default(none) \
+ shared(height, indices, occupency_map, output, width) \
+ num_threads(threads_)
#endif
for (int i = 0; i < static_cast<int>(indices.size ()); ++i)
{
p.getVector3fMap () = input_->points[idx].getVector3fMap ();
p.intensity = response_->points [idx];
-#ifdef _OPENMP
#pragma omp critical
-#endif
{
output.push_back (p);
keypoints_indices_->indices.push_back (idx);
output.is_dense = true;
}
+} // namespace pcl
+
#define PCL_INSTANTIATE_TrajkovicKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::TrajkovicKeypoint3D<T,U,N>;
+
#endif
+
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#pragma once
#ifdef __DEPRECATED
-#warning UniformSampling is not a Keypoint anymore, use <pcl/filters/uniform_sampling.h> instead.
+PCL_DEPRECATED_HEADER(1, 12, "UniformSampling is not a Keypoint anymore, use <pcl/filters/uniform_sampling.h> instead.")
#endif
#include <pcl/filters/uniform_sampling.h>
void
pcl::keypoints::agast::AbstractAgastDetector::detectKeypoints (
const std::vector<unsigned char> & intensity_data,
- pcl::PointCloud<pcl::PointUV> &output)
+ pcl::PointCloud<pcl::PointUV> &output) const
{
detect (&(intensity_data[0]), output.points);
void
pcl::keypoints::agast::AbstractAgastDetector::detectKeypoints (
const std::vector<float> & intensity_data,
- pcl::PointCloud<pcl::PointUV> &output)
+ pcl::PointCloud<pcl::PointUV> &output) const
{
detect (&(intensity_data[0]), output.points);
pcl::keypoints::agast::AbstractAgastDetector::computeCornerScores (
const unsigned char* im,
const std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all,
- std::vector<ScoreIndex> &scores)
+ std::vector<ScoreIndex> &scores) const
{
unsigned int num_corners = static_cast<unsigned int> (corners_all.size ());
pcl::keypoints::agast::AbstractAgastDetector::computeCornerScores (
const float* im,
const std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all,
- std::vector<ScoreIndex> &scores)
+ std::vector<ScoreIndex> &scores) const
{
unsigned int num_corners = static_cast<unsigned int> (corners_all.size ());
was_touched.resize (array_size, false);
std::vector<int> neighbors_to_check;
-# pragma omp parallel for num_threads (parameters_.max_no_of_threads) default (shared) \
- firstprivate (was_touched, neighbors_to_check, angle_histogram) schedule (dynamic, 10)
+#pragma omp parallel for \
+ default(none) \
+ shared(array_size, border_descriptions, interest_image, range_image, radius_reciprocal, radius_squared, scale_idx, \
+ surface_change_directions, surface_change_scores, start_usage_range) \
+ firstprivate(was_touched, neighbors_to_check, angle_histogram) \
+ schedule(dynamic, 10) \
+ num_threads(parameters_.max_no_of_threads)
for (int index=0; index<array_size; ++index)
{
float& interest_value = interest_image[index];
neighbors_within_radius_overhead;
//double interest_value_calculation_start_time = getTime ();
-# pragma omp parallel for default (shared) num_threads (parameters_.max_no_of_threads) schedule (guided, 10) \
- firstprivate (was_touched, neighbors_to_check, angle_histogram, neighbors_within_radius_overhead, \
- angle_elements, relevant_point_still_valid)
+#pragma omp parallel for \
+ default(none) \
+ shared(array_size, border_descriptions, increased_radius_squared, radius_reciprocal, radius_overhead_squared, range_image, search_radius, \
+ surface_change_directions, surface_change_scores) \
+ num_threads(parameters_.max_no_of_threads) \
+ schedule(guided, 10) \
+ firstprivate(was_touched, neighbors_to_check, angle_histogram, neighbors_within_radius_overhead, angle_elements, relevant_point_still_valid)
for (int index=0; index<array_size; ++index)
{
if (interest_image_[index] <= 1.0f)
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/ferns" ${ferns_incs})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/dt" ${dt_impl_incs})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/ferns" ${ferns_impl_incs})
-PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/svm" ${svm_impl_incs})
\ No newline at end of file
+PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/svm" ${svm_impl_incs})
#pragma once
+#include <pcl/ml/pairwise_potential.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
-#include <pcl/ml/pairwise_potential.h>
-
namespace pcl {
class PCL_EXPORTS DenseCrf {
expAndNormalize(std::vector<float>& out,
const std::vector<float>& in,
float scale,
- float relax = 1.0f);
+ float relax = 1.0f) const;
void
expAndNormalizeORI(float* out,
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/dt/decision_tree.h>
#include <istream>
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/dt/decision_forest.h>
#include <pcl/ml/dt/decision_tree_evaluator.h>
#include <pcl/ml/feature_handler.h>
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/dt/decision_forest.h>
#include <pcl/ml/dt/decision_tree.h>
#include <pcl/ml/dt/decision_tree_trainer.h>
#pragma once
-#include <boost/shared_ptr.hpp>
-
#include <pcl/common/common.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_macros.h> // for PCL_EXPORTS
namespace pcl {
template <class FeatureType,
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/dt/decision_tree.h>
#include <pcl/ml/feature_handler.h>
#include <pcl/ml/stats_estimator.h>
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/dt/decision_tree.h>
#include <pcl/ml/dt/decision_tree_data_provider.h>
#include <pcl/ml/feature_handler.h>
*
* \param node_index the index of the node to access
*/
- inline NodeType& operator[](const std::size_t node_index)
+ inline NodeType&
+ operator[](const std::size_t node_index)
{
return nodes_[node_index];
}
*
* \param node_index the index of the node to access
*/
- inline const NodeType& operator[](const std::size_t node_index) const
+ inline const NodeType&
+ operator[](const std::size_t node_index) const
{
return nodes_[node_index];
}
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/feature_handler.h>
#include <pcl/ml/ferns/fern.h>
#include <pcl/ml/stats_estimator.h>
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/feature_handler.h>
#include <pcl/ml/ferns/fern.h>
#include <pcl/ml/stats_estimator.h>
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/dt/decision_forest.h>
#include <pcl/ml/dt/decision_forest_evaluator.h>
#include <pcl/ml/feature_handler.h>
#pragma once
+namespace pcl {
+
template <class FeatureType,
class DataSet,
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
DecisionForestTrainer()
: num_of_trees_to_train_(1), decision_tree_trainer_()
{}
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
~DecisionForestTrainer()
{}
class ExampleIndex,
class NodeType>
void
-pcl::DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
- train(pcl::DecisionForest<NodeType>& forest)
+DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::train(
+ pcl::DecisionForest<NodeType>& forest)
{
for (std::size_t tree_index = 0; tree_index < num_of_trees_to_train_; ++tree_index) {
pcl::DecisionTree<NodeType> tree;
forest.push_back(tree);
}
}
+
+} // namespace pcl
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/dt/decision_tree.h>
#include <pcl/ml/feature_handler.h>
#include <pcl/ml/stats_estimator.h>
#include <vector>
+namespace pcl {
+
template <class FeatureType,
class DataSet,
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
DecisionTreeEvaluator()
{}
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
~DecisionTreeEvaluator()
{}
class ExampleIndex,
class NodeType>
void
-pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
evaluate(pcl::DecisionTree<NodeType>& tree,
pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex>& feature_handler,
pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex>&
class ExampleIndex,
class NodeType>
void
-pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
evaluateAndAdd(
pcl::DecisionTree<NodeType>& tree,
pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex>& feature_handler,
class ExampleIndex,
class NodeType>
void
-pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
evaluate(pcl::DecisionTree<NodeType>& tree,
pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex>& feature_handler,
pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex>&
ExampleIndex example,
NodeType& leave)
{
-
NodeType* node = &(tree.getRoot());
while (!node->sub_nodes.empty()) {
class ExampleIndex,
class NodeType>
void
-pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
getNodes(pcl::DecisionTree<NodeType>& tree,
pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex>& feature_handler,
pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex>&
nodes.push_back(node);
}
}
+
+} // namespace pcl
#pragma once
+namespace pcl {
+
template <class FeatureType,
class DataSet,
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
DecisionTreeTrainer()
: max_tree_depth_(15)
, num_of_features_(1000)
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
~DecisionTreeTrainer()
{}
class ExampleIndex,
class NodeType>
void
-pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
- train(pcl::DecisionTree<NodeType>& tree)
+DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::train(
+ pcl::DecisionTree<NodeType>& tree)
{
// create random features
std::vector<FeatureType> features;
class ExampleIndex,
class NodeType>
void
-pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
trainDecisionTreeNode(std::vector<FeatureType>& features,
std::vector<ExampleIndex>& examples,
std::vector<LabelType>& label_data,
class ExampleIndex,
class NodeType>
void
-pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
createThresholdsUniform(const std::size_t num_of_thresholds,
std::vector<float>& values,
std::vector<float>& thresholds)
min_value + step * (static_cast<float>(threshold_index + 1));
}
}
+
+} // namespace pcl
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/feature_handler.h>
#include <pcl/ml/ferns/fern.h>
#include <pcl/ml/stats_estimator.h>
#include <vector>
+namespace pcl {
+
template <class FeatureType,
class DataSet,
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
- FernEvaluator()
+FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::FernEvaluator()
{}
template <class FeatureType,
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
- ~FernEvaluator()
+FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::~FernEvaluator()
{}
template <class FeatureType,
class ExampleIndex,
class NodeType>
void
-pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluate(
+FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluate(
pcl::Fern<FeatureType, NodeType>& fern,
pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex>& feature_handler,
pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex>& stats_estimator,
class ExampleIndex,
class NodeType>
void
-pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
- evaluateAndAdd(
- pcl::Fern<FeatureType, NodeType>& fern,
- pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex>& feature_handler,
- pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex>&
- stats_estimator,
- DataSet& data_set,
- std::vector<ExampleIndex>& examples,
- std::vector<LabelType>& label_data)
+FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluateAndAdd(
+ pcl::Fern<FeatureType, NodeType>& fern,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex>& feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex>& stats_estimator,
+ DataSet& data_set,
+ std::vector<ExampleIndex>& examples,
+ std::vector<LabelType>& label_data)
{
const std::size_t num_of_examples = examples.size();
const std::size_t num_of_branches = stats_estimator.getNumOfBranches();
class ExampleIndex,
class NodeType>
void
-pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::getNodes(
+FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::getNodes(
pcl::Fern<FeatureType, NodeType>& fern,
pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex>& feature_handler,
pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex>& stats_estimator,
nodes.push_back(&(fern[node_index]));
}
}
+
+} // namespace pcl
#pragma once
+namespace pcl {
+
template <class FeatureType,
class DataSet,
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::FernTrainer()
+FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::FernTrainer()
: fern_depth_(10)
, num_of_features_(1000)
, num_of_thresholds_(10)
class LabelType,
class ExampleIndex,
class NodeType>
-pcl::FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
- ~FernTrainer()
+FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::~FernTrainer()
{}
template <class FeatureType,
class ExampleIndex,
class NodeType>
void
-pcl::FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::train(
+FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::train(
pcl::Fern<FeatureType, NodeType>& fern)
{
const std::size_t num_of_branches = stats_estimator_->getNumOfBranches();
class ExampleIndex,
class NodeType>
void
-pcl::FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
+FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
createThresholdsUniform(const std::size_t num_of_thresholds,
std::vector<float>& values,
std::vector<float>& thresholds)
thresholds[threshold_index] = min_value + step * (threshold_index + 1);
}
}
+
+} // namespace pcl
//#include <stdlib.h>
//#include <time.h>
+namespace pcl {
template <typename PointT>
-
pcl::Kmeans<PointT>::Kmeans() : cluster_field_name_("")
+Kmeans<PointT>::Kmeans() : cluster_field_name_("")
{}
template <typename PointT>
-
pcl::Kmeans<PointT>::~Kmeans()
+Kmeans<PointT>::~Kmeans()
{}
template <typename PointT>
void
-pcl::Kmeans<PointT>::k_means()
-{}
-
-template <typename PointT>
-void
-pcl::Kmeans<PointT>::cluster(std::vector<PointIndices>& clusters)
+Kmeans<PointT>::cluster(std::vector<PointIndices>& clusters)
{
if (!initCompute() || (input_ != 0 && input_->points.empty()) ||
(indices_ != 0 && indices_->empty())) {
deinitCompute();
}
+} // namespace pcl
#define PCL_INSTANTIATE_Kmeans(T) template class PCL_EXPORTS pcl::Kmeans<T>;
#pragma once
-#include <set>
-
#include <pcl/common/io.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_base.h>
+#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
-#include <pcl/pcl_base.h>
-#include <pcl/pcl_macros.h>
+#include <set>
namespace pcl {
#pragma once
#include <pcl/common/common.h>
-
#include <pcl/ml/feature_handler.h>
#include <pcl/ml/multi_channel_2d_comparison_feature.h>
#include <pcl/ml/multi_channel_2d_data_set.h>
#pragma once
-#include <vector>
-
#include <pcl/ml/permutohedral.h>
+#include <vector>
+
namespace pcl {
class PairwisePotential {
#pragma GCC system_header
#endif
-#include <boost/intrusive/hashtable.hpp>
-#include <map>
#include <pcl/common/eigen.h>
-#include <vector>
+#include <pcl/memory.h>
+#include <pcl/pcl_macros.h>
+
+#include <boost/intrusive/hashtable.hpp>
-// TODO: SWAP with Boost intrusive hash table
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
-
-#include <pcl/pcl_macros.h>
+#include <map>
+#include <vector>
namespace pcl {
+
/** Implementation of a high-dimensional gaussian filtering using the permutohedral
* lattice.
*
#pragma once
+#include <pcl/common/eigen.h>
+#include <pcl/ml/svm.h>
+
#include <cctype>
#include <cerrno>
#include <cstdio>
#include <cstring>
#include <fstream>
#include <iostream>
-#include <pcl/common/eigen.h>
#include <vector>
-
-#include <pcl/ml/svm.h>
#define Malloc(type, n) static_cast<type*>(malloc((n) * sizeof(type)))
namespace pcl {
/** Convert the libSVM format (svm_problem) into a easier output format. */
void
- adaptLibSVMToInput(std::vector<SVMData>& training_set, svm_problem prob);
+ adaptLibSVMToInput(std::vector<SVMData>& training_set, svm_problem prob) const;
/** Load a problem from an extern file. */
bool
pcl::DenseCrf::expAndNormalize(std::vector<float>& out,
const std::vector<float>& in,
float scale,
- float relax)
+ float relax) const
{
std::vector<float> V(N_ + 10);
for (int i = 0; i < N_; i++) {
#ifndef _LIBSVM_HPP_
#define _LIBSVM_HPP_
+#include <pcl/ml/svm.h>
+
#include <cctype>
#include <cfloat>
#include <climits>
#include <cstdlib>
#include <cstring>
#include <iostream>
-#include <pcl/ml/svm.h>
int libsvm_version = LIBSVM_VERSION;
using Qfloat = float;
using schar = signed char;
free(model->rho);
free(model->label);
free(model->nSV);
+ free(model->scaling);
free(model);
return nullptr;
}
free(model->rho);
free(model->label);
free(model->nSV);
+ free(model->scaling);
free(model);
return nullptr;
}
free(model->rho);
free(model->label);
free(model->nSV);
+ free(model->scaling);
free(model);
return nullptr;
}
// printf("%d e %f\n",model->scaling[j-2].index,model->scaling[j-2].value);
- if (ferror(fp) != 0 || fclose(fp) != 0)
+ if (ferror(fp) != 0 || fclose(fp) != 0) {
+ free(model->rho);
+ free(model->label);
+ free(model->nSV);
+ free(model->scaling);
+ free(model);
return nullptr;
+ }
model->free_sv = 1; // XXX
#ifndef PCL_SVM_WRAPPER_HPP_
#define PCL_SVM_WRAPPER_HPP_
+#include <pcl/ml/svm_wrapper.h>
+
#include <cassert>
#include <fstream>
-#include <pcl/ml/svm_wrapper.h>
char*
pcl::SVM::readline(FILE* input)
};
void
-pcl::SVM::adaptLibSVMToInput(std::vector<SVMData>& training_set, svm_problem prob)
+pcl::SVM::adaptLibSVMToInput(std::vector<SVMData>& training_set, svm_problem prob) const
{
training_set.clear(); // Reset input
#ifndef PCL_OCTREE_BASE_HPP
#define PCL_OCTREE_BASE_HPP
-#include <vector>
-
#include <pcl/impl/instantiate.hpp>
+#include <vector>
+
namespace pcl {
namespace octree {
//////////////////////////////////////////////////////////////////////////////////////////////
* $Id$
*/
-#ifndef PCL_OCTREE_POINTCLOUD_HPP_
-#define PCL_OCTREE_POINTCLOUD_HPP_
-
-#include <cassert>
+#pragma once
#include <pcl/common/common.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/octree/impl/octree_base.hpp>
+#include <cassert>
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT,
typename LeafContainerT,
pcl::octree::OctreeContainerEmpty, \
pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
pcl::octree::OctreeContainerEmpty>>;
-
-#endif /* OCTREE_POINTCLOUD_HPP_ */
*
*/
-#ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_
-#define PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_
+#pragma once
#include <pcl/common/geometry.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/console/print.h>
+
/*
* OctreePointCloudAdjacency is not precompiled, since it's used in other
* parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT
#define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \
template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
-
-#endif
#include <cassert>
-//////////////////////////////////////////////////////////////////////////////////////////////
+namespace pcl {
+
+namespace octree {
+
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
bool
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- voxelSearch(const PointT& point, std::vector<int>& point_idx_data)
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch(
+ const PointT& point, std::vector<int>& point_idx_data)
{
assert(isFinite(point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
return (b_success);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
bool
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- voxelSearch(const int index, std::vector<int>& point_idx_data)
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch(
+ const int index, std::vector<int>& point_idx_data)
{
const PointT search_point = this->getPointByIndex(index);
return (this->voxelSearch(search_point, point_idx_data));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- nearestKSearch(const PointT& p_q,
- int k,
- std::vector<int>& k_indices,
- std::vector<float>& k_sqr_distances)
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch(
+ const PointT& p_q,
+ int k,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances)
{
assert(this->leaf_count_ > 0);
assert(isFinite(p_q) &&
return static_cast<int>(k_indices.size());
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- nearestKSearch(int index,
- int k,
- std::vector<int>& k_indices,
- std::vector<float>& k_sqr_distances)
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch(
+ int index, int k, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances)
{
const PointT search_point = this->getPointByIndex(index);
return (nearestKSearch(search_point, k, k_indices, k_sqr_distances));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance)
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch(
+ const PointT& p_q, int& result_index, float& sqr_distance)
{
assert(this->leaf_count_ > 0);
assert(isFinite(p_q) &&
return;
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- approxNearestSearch(int query_index, int& result_index, float& sqr_distance)
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch(
+ int query_index, int& result_index, float& sqr_distance)
{
const PointT search_point = this->getPointByIndex(query_index);
return (approxNearestSearch(search_point, result_index, sqr_distance));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- radiusSearch(const PointT& p_q,
- const double radius,
- std::vector<int>& k_indices,
- std::vector<float>& k_sqr_distances,
- unsigned int max_nn) const
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
+ const PointT& p_q,
+ const double radius,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn) const
{
assert(isFinite(p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
return (static_cast<int>(k_indices.size()));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- radiusSearch(int index,
- const double radius,
- std::vector<int>& k_indices,
- std::vector<float>& k_sqr_distances,
- unsigned int max_nn) const
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
+ int index,
+ const double radius,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn) const
{
const PointT search_point = this->getPointByIndex(index);
return (radiusSearch(search_point, radius, k_indices, k_sqr_distances, max_nn));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- boxSearch(const Eigen::Vector3f& min_pt,
- const Eigen::Vector3f& max_pt,
- std::vector<int>& k_indices) const
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearch(
+ const Eigen::Vector3f& min_pt,
+ const Eigen::Vector3f& max_pt,
+ std::vector<int>& k_indices) const
{
OctreeKey key;
return (static_cast<int>(k_indices.size()));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
double
-
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
getKNearestNeighborRecursive(
const PointT& point,
unsigned int K,
return (smallest_squared_dist);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
-
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
getNeighborsWithinRadiusRecursive(const PointT& point,
const double radiusSquared,
const BranchNode* node,
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
-
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
approxNearestSearchRecursive(const PointT& point,
const BranchNode* node,
const OctreeKey& key,
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
float
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- pointSquaredDist(const PointT& point_a, const PointT& point_b) const
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::pointSquaredDist(
+ const PointT& point_a, const PointT& point_b) const
{
return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
- boxSearchRecursive(const Eigen::Vector3f& min_pt,
- const Eigen::Vector3f& max_pt,
- const BranchNode* node,
- const OctreeKey& key,
- unsigned int tree_depth,
- std::vector<int>& k_indices) const
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecursive(
+ const Eigen::Vector3f& min_pt,
+ const Eigen::Vector3f& max_pt,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ std::vector<int>& k_indices) const
{
// iterate over all children
for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
getIntersectedVoxelCenters(Eigen::Vector3f origin,
Eigen::Vector3f direction,
AlignedPointTVector& voxel_center_list,
return (0);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
getIntersectedVoxelIndices(Eigen::Vector3f origin,
Eigen::Vector3f direction,
std::vector<int>& k_indices,
return (0);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
getIntersectedVoxelCentersRecursive(double min_x,
double min_y,
double min_z,
return (voxel_count);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
-
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
getIntersectedVoxelIndicesRecursive(double min_x,
double min_y,
double min_z,
return (voxel_count);
}
+} // namespace octree
+} // namespace pcl
+
#define PCL_INSTANTIATE_OctreePointCloudSearch(T) \
template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
#include <pcl/octree/octree_base.h>
#include <pcl/octree/octree_iterator.h>
#include <pcl/octree/octree_pointcloud.h>
-
#include <pcl/octree/octree_pointcloud_adjacency.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <pcl/octree/octree_pointcloud_density.h>
#include <pcl/octree/octree_pointcloud_pointvector.h>
#include <pcl/octree/octree_pointcloud_singlepoint.h>
#include <pcl/octree/octree_pointcloud_voxelcentroid.h>
-
#include <pcl/octree/octree_search.h>
#pragma once
-#include <vector>
-
#include <pcl/octree/octree_container.h>
#include <pcl/octree/octree_iterator.h>
#include <pcl/octree/octree_key.h>
#include <pcl/octree/octree_nodes.h>
#include <pcl/pcl_macros.h>
+#include <vector>
+
namespace pcl {
namespace octree {
}
/** \brief Get const pointer to container */
- const ContainerT* operator->() const { return &container_; }
+ const ContainerT*
+ operator->() const
+ {
+ return &container_;
+ }
/** \brief Get pointer to container */
- ContainerT* operator->() { return &container_; }
+ ContainerT*
+ operator->()
+ {
+ return &container_;
+ }
/** \brief Get const reference to container */
- const ContainerT& operator*() const { return container_; }
+ const ContainerT&
+ operator*() const
+ {
+ return container_;
+ }
/** \brief Get reference to container */
- ContainerT& operator*() { return container_; }
+ ContainerT&
+ operator*()
+ {
+ return container_;
+ }
/** \brief Get const reference to container */
const ContainerT&
using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
- PCL_DEPRECATED("use leaf_depth_begin() instead")
+ PCL_DEPRECATED(1, 12, "use leaf_depth_begin() instead")
LeafNodeIterator
leaf_begin(unsigned int max_depth_arg = 0)
{
return LeafNodeIterator(this, max_depth_arg);
};
- PCL_DEPRECATED("use leaf_depth_end() instead") const LeafNodeIterator leaf_end()
+ PCL_DEPRECATED(1, 12, "use leaf_depth_end() instead")
+ const LeafNodeIterator
+ leaf_end()
{
return LeafNodeIterator();
};
* \param n_arg: some value
* \return binary logarithm (log2) of argument n_arg
*/
- PCL_DEPRECATED("use std::log2 instead") inline double Log2(double n_arg)
+ PCL_DEPRECATED(1, 12, "use std::log2 instead") inline double Log2(double n_arg)
{
return std::log2(n_arg);
}
#pragma once
-#include <vector>
-
#include <pcl/octree/octree_container.h>
#include <pcl/octree/octree_iterator.h>
#include <pcl/octree/octree_key.h>
#include <pcl/octree/octree_nodes.h>
#include <pcl/pcl_macros.h>
+#include <vector>
+
namespace pcl {
namespace octree {
+
/** \brief Octree class
* \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should
* be initially defined).
using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
- PCL_DEPRECATED("use leaf_depth_begin() instead")
+ PCL_DEPRECATED(1, 12, "use leaf_depth_begin() instead")
LeafNodeIterator
leaf_begin(unsigned int max_depth_arg = 0u)
{
return LeafNodeIterator(this, max_depth_arg ? max_depth_arg : this->octree_depth_);
};
- PCL_DEPRECATED("use leaf_depth_end() instead") const LeafNodeIterator leaf_end()
+ PCL_DEPRECATED(1, 12, "use leaf_depth_end() instead")
+ const LeafNodeIterator
+ leaf_end()
{
return LeafNodeIterator(this, 0, nullptr);
};
{
leaf_count_ = source.leaf_count_;
branch_count_ = source.branch_count_;
+ delete root_node_;
+
root_node_ = new (BranchNode)(*(source.root_node_));
depth_mask_ = source.depth_mask_;
max_key_ = source.max_key_;
createLeaf(const OctreeKey& key_arg)
{
- LeafNode* leaf_node;
+ LeafNode* leaf_node = nullptr;
BranchNode* leaf_node_parent;
createLeafRecursive(key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent);
* \param n_arg: some value
* \return binary logarithm (log2) of argument n_arg
*/
- PCL_DEPRECATED("use std::log2 instead") double Log2(double n_arg)
+ PCL_DEPRECATED(1, 12, "use std::log2 instead") double Log2(double n_arg)
{
return std::log2(n_arg);
}
#pragma once
+#include <cassert>
#include <cstddef>
#include <vector>
-#include <pcl/pcl_macros.h>
-
namespace pcl {
namespace octree {
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Octree container class that can serve as a base to construct own leaf node
* container classes.
#pragma once
-#include <pcl/octree/octree.h>
-
#include <pcl/octree/impl/octree2buf_base.hpp>
#include <pcl/octree/impl/octree_base.hpp>
#include <pcl/octree/impl/octree_iterator.hpp>
#include <pcl/octree/impl/octree_pointcloud.hpp>
#include <pcl/octree/impl/octree_search.hpp>
+#include <pcl/octree/octree.h>
#pragma once
-#include <cstddef>
-#include <deque>
-#include <vector>
-
#include <pcl/octree/octree_key.h>
#include <pcl/octree/octree_nodes.h>
+#include <cstddef>
+#include <deque>
#include <iterator>
+#include <vector>
// Ignore warnings in the above headers
#ifdef __GNUC__
/** \brief *operator.
* \return pointer to the current octree node
*/
- inline OctreeNode* operator*() const
+ inline OctreeNode*
+ operator*() const
{ // return designated object
if (octree_ && current_state_) {
return (current_state_->node_);
/** \brief *operator.
* \return pointer to the current octree leaf node
*/
- OctreeNode* operator*() const
+ OctreeNode*
+ operator*() const
{
// return designated object
OctreeNode* ret = 0;
#pragma once
#include <cstdint>
+#include <cstring> // for memcpy
namespace pcl {
namespace octree {
{}
/** \brief Copy constructor. */
- OctreeKey(const OctreeKey& source) { memcpy(key_, source.key_, sizeof(key_)); }
+ OctreeKey(const OctreeKey& source) { std::memcpy(key_, source.key_, sizeof(key_)); }
OctreeKey&
operator=(const OctreeKey&) = default;
#pragma once
-#include <vector>
-
#include <pcl/pcl_macros.h>
+#include <vector>
+
namespace pcl {
namespace octree {
#pragma once
-#include <cstddef>
-
-#include <cassert>
+#include <pcl/octree/octree_container.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_macros.h>
#include <Eigen/Core>
-#include <pcl/pcl_macros.h>
-
-#include "octree_container.h"
+#include <cassert>
+#include <cstddef>
namespace pcl {
namespace octree {
}
/** \brief Get const pointer to container */
- const ContainerT* operator->() const { return &container_; }
+ const ContainerT*
+ operator->() const
+ {
+ return &container_;
+ }
/** \brief Get pointer to container */
- ContainerT* operator->() { return &container_; }
+ ContainerT*
+ operator->()
+ {
+ return &container_;
+ }
/** \brief Get const reference to container */
- const ContainerT& operator*() const { return container_; }
+ const ContainerT&
+ operator*() const
+ {
+ return container_;
+ }
/** \brief Get reference to container */
- ContainerT& operator*() { return container_; }
+ ContainerT&
+ operator*()
+ {
+ return container_;
+ }
/** \brief Get const reference to container */
const ContainerT&
* \param child_idx_arg: index to child node
* \return OctreeNode pointer
* */
- inline OctreeNode*& operator[](unsigned char child_idx_arg)
+ inline OctreeNode*&
+ operator[](unsigned char child_idx_arg)
{
assert(child_idx_arg < 8);
return child_node_array_[child_idx_arg];
}
/** \brief Get const pointer to container */
- const ContainerT* operator->() const { return &container_; }
+ const ContainerT*
+ operator->() const
+ {
+ return &container_;
+ }
/** \brief Get pointer to container */
- ContainerT* operator->() { return &container_; }
+ ContainerT*
+ operator->()
+ {
+ return &container_;
+ }
/** \brief Get const reference to container */
- const ContainerT& operator*() const { return container_; }
+ const ContainerT&
+ operator*() const
+ {
+ return container_;
+ }
/** \brief Get reference to container */
- ContainerT& operator*() { return container_; }
+ ContainerT&
+ operator*()
+ {
+ return container_;
+ }
/** \brief Get const reference to container */
const ContainerT&
#pragma once
#include <pcl/octree/octree_base.h>
-
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#pragma once
-#include <boost/shared_ptr.hpp>
-
#include <pcl/octree/octree2buf_base.h>
#include <pcl/octree/octree_pointcloud.h>
+#include <pcl/memory.h>
namespace pcl {
namespace octree {
#pragma once
-#include <pcl/point_cloud.h>
-
#include <pcl/octree/octree_pointcloud.h>
+#include <pcl/point_cloud.h>
namespace pcl {
namespace octree {
+
/** \brief @b Octree pointcloud search class
* \note This class provides several methods for spatial neighbor search based on octree
* structure
prioPointQueueEntry() : point_idx_(0), point_distance_(0) {}
/** \brief Constructor for initializing priority queue entry.
- * \param[in] point_idx an index representing a point in the dataset given by \a
- * setInputCloud \param[in] point_distance distance of query point to voxel center
+ * \param[in] point_idx index for a dataset point given by \a setInputCloud
+ * \param[in] point_distance distance of query point to voxel center
*/
prioPointQueueEntry(unsigned int& point_idx, float point_distance)
: point_idx_(point_idx), point_distance_(point_distance)
////////////////////////////////////////////////////////////////////////////////
template<typename ContainerT, typename PointT> void
- OutofcoreOctreeBase<ContainerT, PointT>::DeAllocEmptyNodeCache (OutofcoreOctreeBaseNode<ContainerT, PointT>* current)
+ OutofcoreOctreeBase<ContainerT, PointT>::DeAllocEmptyNodeCache (OutofcoreOctreeBaseNode<ContainerT, PointT>* current)
{
+ if (current == nullptr)
+ current = root_node_;
+
if (current->size () == 0)
{
current->flush_DeAlloc_this_only ();
{
DeAllocEmptyNodeCache (current->children[i]);
}
-
}
////////////////////////////////////////////////////////////////////////////////
for (std::size_t i = 0; i < 8; i++)
{
- if (children_[i])
- {
- OutofcoreOctreeBaseNode<ContainerT, PointT>* current = children_[i];
- delete (current);
- }
+ delete static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(children_[i]);
}
children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (nullptr));
num_children_ = 0;
bool
checkExtension (const boost::filesystem::path& path_name);
-
- /** \brief DEPRECATED - Flush all nodes' cache
- * \deprecated this was moved to the octree_node class
- */
+ /** \brief Flush all nodes' cache */
void
flushToDisk ();
- /** \brief DEPRECATED - Flush all non leaf nodes' cache
- * \deprecated
- */
+ /** \brief Flush all non leaf nodes' cache */
void
flushToDiskLazy ();
- /** \brief DEPRECATED - Flush empty nodes only
- * \deprecated
- */
+ /** \brief Flush empty nodes only */
void
DeAllocEmptyNodeCache ();
void
loadFromFile (const boost::filesystem::path &path, OutofcoreOctreeBaseNode* super);
- /** \brief Recursively converts data files to ascii XZY files
- * \note This will be deprecated soon
- */
+ /** \brief Recursively converts data files to ascii XZY files */
void
convertToXYZRecursive ();
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/outofcore/boost.h>
#include <pcl/outofcore/cJSON.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/outofcore/boost.h>
#include <pcl/outofcore/cJSON.h>
// PCL
#include <pcl/outofcore/visualization/object.h>
#include <pcl/common/eigen.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
// VTK
#include <thread>
// PCL
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
}
int
- getLodPixelThreshold ()
+ getLodPixelThreshold () const
{
return lod_pixel_threshold_;
}
// PCL - visualziation
//#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/common/common.h>
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
#include <pcl/visualization/vtk/vtkVertexBufferObjectMapper.h>
+#endif
//#include "vtkVBOPolyDataMapper.h"
#pragma once
-#include <pcl/pcl_macros.h>
+#include <pcl/pcl_macros.h> // export macro
namespace pcl
{
#include <pcl/people/hog.h>
-#include <cstring>
+#include <cstring> // for memcpy
+#include <algorithm> // for std::min
#if defined(__SSE2__)
-#include <pcl/sse.h>
+ #include <pcl/sse.h> // sse methods
#else
-#include <cstdlib>
+ #include <cstdlib>
#endif
/** \brief Constructor. */
{
const int hb=h/bin_size, wb=w/bin_size, h0=hb*bin_size, w0=wb*bin_size, nb=wb*hb;
const float s=(float)bin_size, sInv=1/s, sInv2=1/s/s;
- float *H0, *H1, *M0, *M1; int x, y; int *O0, *O1;
+ float *H0, *H1, *M0, *M1; int *O0, *O1;
O0=(int*)alMalloc(h*sizeof(int),16); M0=(float*) alMalloc(h*sizeof(float),16);
O1=(int*)alMalloc(h*sizeof(int),16); M1=(float*) alMalloc(h*sizeof(float),16);
// main loop
float xb = 0;
float init = 0;
- for( x=0; x<w0; x++ ) {
+ for(int x=0; x<w0; x++ ) {
// compute target orientation bins for entire column - very fast
gradQuantize( O+x*h, M+x*h, O0, O1, M0, M1, n_orients, nb, h0, sInv2 );
if( !soft_bin || bin_size==1 ) {
// interpolate w.r.t. orientation only, not spatial bin_size
- H1=H+(x/bin_size)*hb;
- const auto GH = [&H1, &O0, &O1, &y, &M0, &M1]()
- {
- H1[O0[y]]+=M0[y]; H1[O1[y]]+=M1[y]; y++;
+ H1 = H + (x / bin_size) * hb;
+
+ const auto GH = [&H1, &O0, &O1, &M0, &M1](int y) {
+ H1[O0[y]] += M0[y];
+ H1[O1[y]] += M1[y];
};
- if( bin_size==1 ) for(y=0; y<h0;) { GH(); H1++; }
- else if( bin_size==2 ) for(y=0; y<h0;) { GH(); GH(); H1++; }
- else if( bin_size==3 ) for(y=0; y<h0;) { GH(); GH(); GH(); H1++; }
- else if( bin_size==4 ) for(y=0; y<h0;) { GH(); GH(); GH(); GH(); H1++; }
- else for( y=0; y<h0;) { for( int y1=0; y1<bin_size; y1++ ) { GH(); } H1++; }
+ for (int y = 0; y < h0;) {
+ for (int inner_loop = 0; inner_loop < bin_size; ++inner_loop, ++y) {
+ GH(y);
+ }
+ H1++;
+ }
} else {
// interpolate using trilinear interpolation
#if defined(__SSE2__)
bool hasLf, hasRt; int xb0, yb0;
if( x==0 ) { init=(0+.5f)*sInv-0.5f; xb=init; }
hasLf = xb>=0; xb0 = hasLf?(int)xb:-1; hasRt = xb0 < wb-1;
- xd=xb-xb0; xb+=sInv; yb=init; y=0;
+ xd=xb-xb0; xb+=sInv; yb=init;
+ int y=0;
// lambda for code conciseness
// @TODO: remove the very generic closure for specific variable one
const auto GHinit = [&]()
bool hasLf, hasRt; int xb0, yb0;
if( x==0 ) { init=(0+.5f)*sInv-0.5f; xb=init; }
hasLf = xb>=0; xb0 = hasLf?(int)xb:-1; hasRt = xb0 < wb-1;
- xd=xb-xb0; xb+=sInv; yb=init; y=0;
+ xd=xb-xb0; xb+=sInv; yb=init; int y=0;
// lambda for code conciseness
const auto GHinit = [&]()
{
termination_criteria_chain::reset();
}
- int second_guess() { return second_guess_m; }
- int iteration() { return total_iterations_m; }
- int resets() { return resets_m; }
+ int second_guess() const { return second_guess_m; }
+ int iteration() const { return total_iterations_m; }
+ int resets() const { return resets_m; }
protected:
gol_type best_cost_m;
+++ /dev/null
-#include <pcl/recognition/ransac_based/auxiliary.h>
-#error "Using pcl/recognition/auxiliary.h is deprecated, please use pcl/recognition/ransac_based/auxiliary.h instead."
\ No newline at end of file
+++ /dev/null
-#include <pcl/recognition/ransac_based/bvh.h>
-#error "Using pcl/recognition/bvh.h is deprecated, please use pcl/recognition/ransac_based/bvh.h instead."
\ No newline at end of file
#include <pcl/recognition/cg/correspondence_grouping.h>
#include <pcl/recognition/boost.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#pragma once
#include <pcl/features/integral_image2D.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <Eigen/Core>
#pragma once
+#include <pcl/memory.h>
+#include <pcl/common/common.h>
+#include <pcl/ml/dt/decision_tree_data_provider.h>
+#include <pcl/recognition/face_detection/face_common.h>
+
+#include <boost/algorithm/string.hpp>
+#include <boost/filesystem/operations.hpp>
+
#include <iostream>
#include <fstream>
#include <string>
-#include <boost/filesystem/operations.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/algorithm/string.hpp>
-
-#include <pcl/common/common.h>
-#include <pcl/recognition/face_detection/face_common.h>
-#include <pcl/ml/dt/decision_tree_data_provider.h>
namespace bf = boost::filesystem;
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
*
*/
+
#ifndef PCL_RECOGNITION_CORRESPONDENCE_GROUPING_IMPL_H_
#define PCL_RECOGNITION_CORRESPONDENCE_GROUPING_IMPL_H_
+
+namespace pcl
+{
+
template <typename PointModelT, typename PointSceneT> void
-
pcl::CorrespondenceGrouping<PointModelT, PointSceneT>::cluster (std::vector<Correspondences> &clustered_corrs)
+CorrespondenceGrouping<PointModelT, PointSceneT>::cluster (std::vector<Correspondences> &clustered_corrs)
{
clustered_corrs.clear ();
corr_group_scale_.clear ();
deinitCompute ();
}
+} // namespace pcl
+
#endif // PCL_RECOGNITION_CORRESPONDENCE_GROUPING_IMPL_H_
+
{
pcl::ScopeTime tcues ("Computing clutter cues");
-#pragma omp parallel for schedule(dynamic, 4) num_threads(omp_get_num_procs())
+#pragma omp parallel for \
+ default(none) \
+ schedule(dynamic, 4) \
+ num_threads(omp_get_num_procs())
for (int j = 0; j < static_cast<int> (recognition_models_.size ()); j++)
computeClutterCue (recognition_models_[j]);
}
#include "../implicit_shape_model.h"
+#include <pcl/memory.h> // for dynamic_pointer_cast
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
pcl::features::ISMVoteList<PointT>::ISMVoteList () :
feature_estimator_->setSearchMethod (tree);
// typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
-//
boost::dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
+// dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
// feat_est_norm->setInputNormals (normal_cloud);
typename pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
-
boost::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
+ dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
feat_est_norm->setInputNormals (normal_cloud);
feature_estimator_->compute (*feature_cloud);
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
*
- * All rights reserved.
+ * All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
#include <pcl/point_cloud.h>
#include <limits>
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointXYZT, typename PointRGBT> bool
-
pcl::LineRGBD<PointXYZT, PointRGBT>::readLTMHeader (int fd, pcl::io::TARHeader &header)
+LineRGBD<PointXYZT, PointRGBT>::readLTMHeader (int fd, pcl::io::TARHeader &header)
{
// Read in the header
- int result = static_cast<int> (::read (fd, reinterpret_cast<char*> (&header.file_name[0]), 512));
+ int result = static_cast<int> (io::raw_read (fd, reinterpret_cast<char*> (&header.file_name[0]), 512));
if (result == -1)
return (false);
- // We only support regular files for now.
- // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices,
+ // We only support regular files for now.
+ // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices,
// directories, and named pipes.
if (header.file_type[0] != '0' && header.file_type[0] != '\0')
return (false);
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointXYZT, typename PointRGBT> bool
-
pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const std::size_t object_id)
+LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const std::size_t object_id)
{
// Open the file
int ltm_fd = io::raw_open(file_name.c_str (), O_RDONLY);
if (ltm_fd == -1)
return (false);
-
+
int ltm_offset = 0;
pcl::io::TARHeader ltm_header;
unsigned int fsize = ltm_header.getFileSize ();
char *buffer = new char[fsize];
- int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
+ int result = static_cast<int> (io::raw_read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
if (result == -1)
{
delete [] buffer;
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointXYZT, typename PointRGBT> int
-
pcl::LineRGBD<PointXYZT, PointRGBT>::createAndAddTemplate (
+LineRGBD<PointXYZT, PointRGBT>::createAndAddTemplate (
pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
const std::size_t object_id,
const MaskMap & mask_xyz,
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointXYZT, typename PointRGBT> bool
-
pcl::LineRGBD<PointXYZT, PointRGBT>::addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud<pcl::PointXYZRGBA> & cloud, std::size_t object_id)
+LineRGBD<PointXYZT, PointRGBT>::addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud<pcl::PointXYZRGBA> & cloud, std::size_t object_id)
{
// add point cloud
template_point_clouds_.resize (template_point_clouds_.size () + 1);
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointXYZT, typename PointRGBT> void
-
pcl::LineRGBD<PointXYZT, PointRGBT>::detect (
+
+template <typename PointXYZT, typename PointRGBT> void
+LineRGBD<PointXYZT, PointRGBT>::detect (
std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections)
{
std::vector<pcl::QuantizableModality*> modalities;
detection.response = linemod_detection.score;
// compute bounding box:
- // we assume that the bounding boxes of the templates are relative to the center of mass
+ // we assume that the bounding boxes of the templates are relative to the center of mass
// of the template points; so we also compute the center of mass of the points
- // covered by the
+ // covered by the
- const pcl::SparseQuantizedMultiModTemplate & linemod_template =
+ const pcl::SparseQuantizedMultiModTemplate & linemod_template =
linemod_.getTemplate (linemod_detection.template_id);
const std::size_t start_x = std::max (linemod_detection.x, 0);
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointXYZT, typename PointRGBT> void
-
pcl::LineRGBD<PointXYZT, PointRGBT>::detectSemiScaleInvariant (
+
+template <typename PointXYZT, typename PointRGBT> void
+LineRGBD<PointXYZT, PointRGBT>::detectSemiScaleInvariant (
std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections,
const float min_scale,
const float max_scale,
detection.response = linemod_detection.score;
// compute bounding box:
- // we assume that the bounding boxes of the templates are relative to the center of mass
+ // we assume that the bounding boxes of the templates are relative to the center of mass
// of the template points; so we also compute the center of mass of the points
- // covered by the
+ // covered by the
- const pcl::SparseQuantizedMultiModTemplate & linemod_template =
+ const pcl::SparseQuantizedMultiModTemplate & linemod_template =
linemod_.getTemplate (linemod_detection.template_id);
const std::size_t start_x = std::max (linemod_detection.x, 0);
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointXYZT, typename PointRGBT> void
-
pcl::LineRGBD<PointXYZT, PointRGBT>::computeTransformedTemplatePoints (
+
+template <typename PointXYZT, typename PointRGBT> void
+LineRGBD<PointXYZT, PointRGBT>::computeTransformedTemplatePoints (
const std::size_t detection_id, pcl::PointCloud<pcl::PointXYZRGBA> &cloud)
{
if (detection_id >= detections_.size ())
const pcl::BoundingBoxXYZ & template_bounding_box = bounding_boxes_[template_id];
const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box;
- //std::cerr << "detection: "
+ //std::cerr << "detection: "
// << detection_bounding_box.x << ", "
// << detection_bounding_box.y << ", "
// << detection_bounding_box.z << std::endl;
- //std::cerr << "template: "
+ //std::cerr << "template: "
// << template_bounding_box.x << ", "
// << template_bounding_box.y << ", "
// << template_bounding_box.z << std::endl;
const float translation_y = detection_bounding_box.y - template_bounding_box.y;
const float translation_z = detection_bounding_box.z - template_bounding_box.z;
- //std::cerr << "translation: "
+ //std::cerr << "translation: "
// << translation_x << ", "
// << translation_y << ", "
// << translation_z << std::endl;
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointXYZT, typename PointRGBT> void
-
pcl::LineRGBD<PointXYZT, PointRGBT>::refineDetectionsAlongDepth ()
+
+template <typename PointXYZT, typename PointRGBT> void
+LineRGBD<PointXYZT, PointRGBT>::refineDetectionsAlongDepth ()
{
const std::size_t nr_detections = detections_.size ();
for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
}
std::vector<std::size_t> integral_depth_bins (nr_bins, 0);
-
+
integral_depth_bins[0] = depth_bins[0];
for (std::size_t bin_index = 1; bin_index < nr_bins; ++bin_index)
{
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointXYZT, typename PointRGBT> void
-
pcl::LineRGBD<PointXYZT, PointRGBT>::applyProjectiveDepthICPOnDetections ()
+
+template <typename PointXYZT, typename PointRGBT> void
+LineRGBD<PointXYZT, PointRGBT>::applyProjectiveDepthICPOnDetections ()
{
const std::size_t nr_detections = detections_.size ();
for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
const std::size_t start_y = detection.region.y;
const std::size_t pc_width = point_cloud.width;
const std::size_t pc_height = point_cloud.height;
-
+
std::vector<std::pair<float, float> > depth_matches;
for (std::size_t row_index = 0; row_index < pc_height; ++row_index)
{
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointXYZT, typename PointRGBT> void
-
pcl::LineRGBD<PointXYZT, PointRGBT>::removeOverlappingDetections ()
+
+template <typename PointXYZT, typename PointRGBT> void
+LineRGBD<PointXYZT, PointRGBT>::removeOverlappingDetections ()
{
// compute overlap between each detection
const std::size_t nr_detections = detections_.size ();
overlaps (detection_index_1, detection_index_2) = 0.0f;
else
overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume (
- detections_[detection_index_1].bounding_box,
+ detections_[detection_index_1].bounding_box,
detections_[detection_index_2].bounding_box) / bounding_box_volume;
}
}
for (std::size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id)
{
std::vector<std::size_t> & cluster = clusters[cluster_id];
-
+
float average_center_x = 0.0f;
float average_center_y = 0.0f;
float average_center_z = 0.0f;
detections_ = clustered_detections;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointXYZT, typename PointRGBT> float
-
pcl::LineRGBD<PointXYZT, PointRGBT>::computeBoundingBoxIntersectionVolume (
+
+template <typename PointXYZT, typename PointRGBT> float
+LineRGBD<PointXYZT, PointRGBT>::computeBoundingBoxIntersectionVolume (
const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
{
const float x1_min = box1.x;
const float x2_max = box2.x + box2.width;
const float y2_max = box2.y + box2.height;
const float z2_max = box2.z + box2.depth;
-
+
const float xi_min = std::max (x1_min, x2_min);
const float yi_min = std::max (y1_min, y2_min);
const float zi_min = std::max (z1_min, z2_min);
return (intersection_volume);
}
+} // namespace pcl
-#endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
+#endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
#include <algorithm>
#include <cmath>
-//===============================================================================================================================
+
+namespace pcl
+{
+
+namespace recognition
+{
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::Node ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::Node ()
: data_ (nullptr),
parent_ (nullptr),
children_(nullptr)
{}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::~Node ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::~Node ()
{
this->deleteChildren ();
this->deleteData ();
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setCenter (const Scalar *c)
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setCenter (const Scalar *c)
{
center_[0] = c[0];
center_[1] = c[1];
center_[2] = c[2];
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setBounds (const Scalar *b)
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setBounds (const Scalar *b)
{
bounds_[0] = b[0];
bounds_[1] = b[1];
bounds_[5] = b[5];
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::computeRadius ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::computeRadius ()
{
Scalar v[3] = {static_cast<Scalar> (0.5)*(bounds_[1]-bounds_[0]),
static_cast<Scalar> (0.5)*(bounds_[3]-bounds_[2]),
radius_ = static_cast<Scalar> (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline bool
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::createChildren ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::createChildren ()
{
if ( children_ )
return (false);
return (true);
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteChildren ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteChildren ()
{
- if ( children_ )
- {
- delete[] children_;
- children_ = nullptr;
- }
+ delete[] children_;
+ children_ = nullptr;
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteData ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteData ()
{
- if ( data_ )
- {
- delete data_;
- data_ = nullptr;
- }
+ delete data_;
+ data_ = nullptr;
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::makeNeighbors (Node* node)
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::makeNeighbors (Node* node)
{
if ( !this->hasData () || !node->hasData () )
return;
node->full_leaf_neighbors_.insert (this);
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::SimpleOctree ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::SimpleOctree ()
: tree_levels_ (0),
root_ (nullptr)
{
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::~SimpleOctree ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::~SimpleOctree ()
{
this->clear ();
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::clear ()
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::clear ()
{
- if ( root_ )
- {
- delete root_;
- root_ = nullptr;
- }
+ delete root_;
+ root_ = nullptr;
full_leaves_.clear();
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::build (const Scalar* bounds, Scalar voxel_size,
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::build (const Scalar* bounds, Scalar voxel_size,
NodeDataCreator* node_data_creator)
{
if ( voxel_size <= 0 )
root_->computeRadius ();
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
-typename pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::createLeaf (Scalar x, Scalar y, Scalar z)
+typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::createLeaf (Scalar x, Scalar y, Scalar z)
{
// Make sure that the input point is within the octree bounds
if ( x < bounds_[0] || x > bounds_[1] ||
return (node);
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
-typename pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (int i, int j, int k)
+typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (int i, int j, int k)
{
Scalar offset = 0.5f*voxel_size_;
Scalar p[3] = {bounds_[0] + offset + static_cast<Scalar> (i)*voxel_size_,
return (this->getFullLeaf (p[0], p[1], p[2]));
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
-typename pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (Scalar x, Scalar y, Scalar z)
+typename SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (Scalar x, Scalar y, Scalar z)
{
// Make sure that the input point is within the octree bounds
if ( x < bounds_[0] || x > bounds_[1] ||
return (node);
}
-//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
-pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::insertNeighbors (Node* node)
+SimpleOctree<NodeData, NodeDataCreator, Scalar>::insertNeighbors (Node* node)
{
const Scalar* c = node->getCenter ();
Scalar s = static_cast<Scalar> (0.5)*voxel_size_;
neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
}
-//===============================================================================================================================
+} // namespace recognition
+} // namespace pcl
#endif /* SIMPLE_OCTREE_HPP_ */
+
*
*/
+
#ifndef PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_
#define PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_
+
+namespace pcl
+{
+
+namespace recognition
+{
+
template<class T, typename REAL> inline void
-pcl::recognition::VoxelStructure<T,REAL>::build (const REAL bounds[6], int num_of_voxels[3])
+VoxelStructure<T,REAL>::build (const REAL bounds[6], int num_of_voxels[3])
{
this->clear();
min_center_[2] = bounds_[4] + static_cast<REAL> (0.5)*spacing_[2];
}
-//================================================================================================================================
template<class T, typename REAL> inline T*
-pcl::recognition::VoxelStructure<T,REAL>::getVoxel (const REAL p[3])
+VoxelStructure<T,REAL>::getVoxel (const REAL p[3])
{
if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] )
return nullptr;
return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x];
}
-//================================================================================================================================
template<class T, typename REAL> inline T*
-pcl::recognition::VoxelStructure<T,REAL>::getVoxel (int x, int y, int z) const
+VoxelStructure<T,REAL>::getVoxel (int x, int y, int z) const
{
if ( x < 0 || x >= num_of_voxels_[0] ) return nullptr;
if ( y < 0 || y >= num_of_voxels_[1] ) return nullptr;
return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x];
}
-//================================================================================================================================
template<class T, typename REAL> inline int
-pcl::recognition::VoxelStructure<T,REAL>::getNeighbors (const REAL* p, T **neighs) const
+VoxelStructure<T,REAL>::getNeighbors (const REAL* p, T **neighs) const
{
if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] )
return 0;
return num_neighs;
}
-//================================================================================================================================
+} // namespace recognition
+} // namespace pcl
#endif // PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_
+
#include <fstream>
#include <limits>
#include <Eigen/src/Core/Matrix.h>
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
/** \brief Stores square distances to the corresponding neighbours. */
std::vector<float> k_sqr_dist_;
};
-
+
/** \brief The assignment of this structure is to store the statistical/learned weights and other information
* of the trained Implict Shape Model algorithm.
*/
int flags,
Eigen::MatrixXf& cluster_centers);
- /** \brief Generates centers for clusters as described in
+ /** \brief Generates centers for clusters as described in
* Arthur, David and Sergei Vassilvitski (2007) k-means++: The Advantages of Careful Seeding.
* \param[in] data points to cluster
* \param[out] out_centers it will contain generated centers
return (data_.data());
}
- PCL_DEPRECATED("Use new version diff getDifferenceMask(mask0, mask1)")
+ PCL_DEPRECATED(1, 12, "Use new version diff getDifferenceMask(mask0, mask1)")
static void
getDifferenceMask(const MaskMap& mask0, const MaskMap& mask1, MaskMap& diff_mask);
+++ /dev/null
-#include <pcl/recognition/ransac_based/model_library.h>
-#error "Using pcl/recognition/model_library.h is deprecated, please use pcl/recognition/ransac_based/model_library.h instead."
\ No newline at end of file
+++ /dev/null
-#include <pcl/recognition/ransac_based/obj_rec_ransac.h>
-#error "Using pcl/recognition/obj_rec_ransac.h is deprecated, please use pcl/recognition/ransac_based/obj_rec_ransac.h instead."
\ No newline at end of file
+++ /dev/null
-#include <pcl/recognition/ransac_based/orr_graph.h>
-#error "Using pcl/recognition/orr_graph.h is deprecated, please use pcl/recognition/ransac_based/orr_graph.h instead."
\ No newline at end of file
+++ /dev/null
-#include <pcl/recognition/ransac_based/orr_octree.h>
-#error "Using pcl/recognition/orr_octree.h is deprecated, please use pcl/recognition/ransac_based/orr_octree.h instead."
\ No newline at end of file
+++ /dev/null
-#include <pcl/recognition/ransac_based/orr_octree_zprojection.h>
-#error "Using pcl/recognition/orr_octree_zprojection.h is deprecated, please use pcl/recognition/ransac_based/orr_octree_zprojection.h instead."
\ No newline at end of file
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
};
PCL_MAKE_ALIGNED_OPERATOR_NEW
- inline bool operator< (const GradientXY & rhs)
+ inline bool operator< (const GradientXY & rhs) const
{
return (magnitude > rhs.magnitude);
}
}
virtual ~Node ()
- {
- if ( children_[0] )
{
delete children_[0];
delete children_[1];
}
- }
bool
hasChildren () const
/** \brief Frees the memory allocated by this object. After that, you have to call build to use the tree again. */
void
clear()
- {
- if ( root_ )
{
delete root_;
root_ = nullptr;
}
- }
inline const std::vector<BoundedObject*>*
getInputObjects () const
/** \brief Computes the "radius" of the node which is half the diagonal length. */
inline float
- getRadius (){ return radius_;}
+ getRadius () const{ return radius_;}
bool
createChildren ();
inline void
deleteChildren ()
- {
- if ( children_ )
{
delete[] children_;
children_ = nullptr;
}
- }
inline void
deleteData ()
- {
- if ( data_ )
{
delete data_;
data_ = nullptr;
}
- }
/** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens
* of either of the nodes has no data. */
/** \brief Release the memory allocated for the voxels. */
inline void
- clear (){ if ( voxels_ ){ delete[] voxels_; voxels_ = nullptr;}}
+ clear (){ delete[] voxels_; voxels_ = nullptr;}
/** \brief Returns a pointer to the voxel which contains p or NULL if p is not inside the structure. */
inline T*
+++ /dev/null
-#include <pcl/recognition/ransac_based/rigid_transform_space.h>
-#error "Using pcl/recognition/rigid_transform_space.h is deprecated, please use pcl/recognition/ransac_based/rigid_transform_space.h instead."
\ No newline at end of file
+++ /dev/null
-#include <pcl/recognition/ransac_based/simple_octree.h>
-#error "Using pcl/recognition/simple_octree.h is deprecated, please use pcl/recognition/ransac_based/simple_octree.h instead."
\ No newline at end of file
* \param[in] base the feature to compare to.
*/
bool
- compareForEquality (const QuantizedMultiModFeature & base)
+ compareForEquality (const QuantizedMultiModFeature & base) const
{
if (base.x != x)
return false;
+++ /dev/null
-#include <pcl/recognition/ransac_based/trimmed_icp.h>
-#error "Using pcl/recognition/trimmed_icp.h is deprecated, please use pcl/recognition/ransac_based/trimmed_icp.h instead."
\ No newline at end of file
+++ /dev/null
-#include <pcl/recognition/ransac_based/voxel_structure.h>
-#error "Using pcl/recognition/voxel_structure.h is deprecated, please use pcl/recognition/ransac_based/voxel_structure.h instead."
\ No newline at end of file
#include "pcl/recognition/face_detection/face_detector_data_provider.h"
#include "pcl/recognition/face_detection/face_common.h"
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/common/time.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/io/pcd_io.h>
#include "pcl/recognition/face_detection/rf_face_detector_trainer.h"
#include "pcl/recognition/face_detection/face_common.h"
#include "pcl/io/pcd_io.h"
+#include <pcl/memory.h> // for dynamic_pointer_cast
#include "pcl/ml/dt/decision_tree_trainer.h"
#include "pcl/ml/dt/decision_tree_evaluator.h"
#include "pcl/ml/dt/decision_forest_trainer.h"
#include <pcl/recognition/hv/hv_papazov.h>
#include <pcl/features/normal_3d.h>
+
void pcl::RFFaceDetectorTrainer::trainWithDataProvider()
{
dtdp->initialize (directory_);
- auto cast_dtdp =
boost::dynamic_pointer_cast<pcl::DecisionTreeTrainerDataProvider<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, float, int, NodeType>> (dtdp);
+ auto cast_dtdp = dynamic_pointer_cast<pcl::DecisionTreeTrainerDataProvider<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, float, int, NodeType>> (dtdp);
dft.setDecisionTreeDataProvider (cast_dtdp);
pcl::DecisionForest<NodeType> forest;
#include <pcl/recognition/mask_map.h>
+#include <algorithm> // for std::transform
#include <cassert>
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::recognition::ORROctree::clear ()
{
- if ( root_ )
- {
- delete root_;
- root_ = nullptr;
- }
+ delete root_;
+ root_ = nullptr;
full_leaves_.clear();
}
{
// Delete pixel by pixel
for ( int j = 0 ; j < num_pixels_y_ ; ++j )
- if ( pixels_[i][j] )
delete pixels_[i][j];
// Delete the whole row
for ( int i = 0 ; i < num_pixels_x_ ; ++i )
{
for ( int j = 0 ; j < num_pixels_y_ ; ++j )
- if ( sets_[i][j] )
delete sets_[i][j];
delete[] sets_[i];
};
}
+namespace BFGSSpace {
+ enum Status {
+ NegativeGradientEpsilon = -3,
+ NotStarted = -2,
+ Running = -1,
+ Success = 0,
+ NoProgress = 1
+ };
+}
+
template<typename _Scalar, int NX=Eigen::Dynamic>
struct BFGSDummyFunctor
{
virtual double operator() (const VectorType &x) = 0;
virtual void df(const VectorType &x, VectorType &df) = 0;
virtual void fdf(const VectorType &x, Scalar &f, VectorType &df) = 0;
+ virtual BFGSSpace::Status checkGradient(const VectorType& g) { return BFGSSpace::NotStarted; };
};
-namespace BFGSSpace {
- enum Status {
- NegativeGradientEpsilon = -3,
- NotStarted = -2,
- Running = -1,
- Success = 0,
- NoProgress = 1
- };
-}
-
/**
* BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving
* unconstrained nonlinear optimization problems.
BFGSSpace::Status minimize(FVectorType &x);
BFGSSpace::Status minimizeInit(FVectorType &x);
BFGSSpace::Status minimizeOneStep(FVectorType &x);
- BFGSSpace::Status testGradient(Scalar epsilon);
+ BFGSSpace::Status testGradient();
+ PCL_DEPRECATED(1, 13, "Use `testGradient()` instead")
+ BFGSSpace::Status testGradient(Scalar) { return testGradient(); }
void resetParameters(void) { parameters = Parameters(); }
Parameters parameters;
return BFGSSpace::Success;
}
-template<typename FunctorType> typename BFGSSpace::Status
-BFGS<FunctorType>::testGradient(Scalar epsilon)
+template <typename FunctorType>
+typename BFGSSpace::Status
+BFGS<FunctorType>::testGradient()
{
- if(epsilon < 0)
- return BFGSSpace::NegativeGradientEpsilon;
- else
- {
- if(gradient.norm () < epsilon)
- return BFGSSpace::Success;
- else
- return BFGSSpace::Running;
- }
+ return functor.checkGradient(gradient);
}
template<typename FunctorType> typename BFGS<FunctorType>::Scalar
#include <boost/property_map/property_map.hpp>
#include <boost/noncopyable.hpp>
-#include <boost/make_shared.hpp>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
#include <pcl/pcl_base.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/correspondence_types.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/correspondence_estimation.h>
virtual ~CorrespondenceRejector () {}
/** \brief Provide a pointer to the vector of the input correspondences.
- * \param[in] correspondences the const boost shared pointer to a correspondence vector
+ * \param[in] correspondences the const shared pointer to a correspondence vector
*/
virtual inline void
setInputCorrespondences (const CorrespondencesConstPtr &correspondences)
};
/** \brief Get a pointer to the vector of the input correspondences.
- * \return correspondences the const boost shared pointer to a correspondence vector
+ * \return correspondences the const shared pointer to a correspondence vector
*/
inline CorrespondencesConstPtr
getInputCorrespondences () { return input_correspondences_; };
#pragma once
#include <pcl/registration/correspondence_rejection.h>
+#include <pcl/memory.h> // for static_pointer_cast
+
namespace pcl
{
/** \brief Get the maximum distance used for thresholding in correspondence rejection. */
inline float
- getMaximumDistance () { return std::sqrt (max_distance_); };
+ getMaximumDistance () const { return std::sqrt (max_distance_); };
/** \brief Provide a source point cloud dataset (must contain XYZ
* data!), used to compute the correspondence distance.
* \param[in] cloud a cloud containing XYZ data
*/
- template <typename PointT> inline void
+ template <typename PointT>
+ PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorDistance::setInputCloud is deprecated. Please use setInputSource instead")
+ inline void
setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
{
- PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
}
/** \brief Provide a source point cloud dataset (must contain XYZ
{
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
}
/** \brief Provide a target point cloud dataset (must contain XYZ
{
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
}
setSearchMethodTarget (const typename pcl::search::KdTree<PointT>::Ptr &tree,
bool force_no_recompute = false)
{
-
boost::static_pointer_cast< DataContainer<PointT> >
+ static_pointer_cast< DataContainer<PointT> >
(data_container_)->setSearchMethodTarget (tree, force_no_recompute );
}
#pragma once
#include <pcl/registration/correspondence_rejection.h>
+#include <pcl/memory.h> // for static_pointer_cast
#include <pcl/point_cloud.h>
+
namespace pcl
{
namespace registration
{
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
}
/** \brief Provide a source point cloud dataset (must contain XYZ
* data!), used to compute the correspondence distance.
* \param[in] cloud a cloud containing XYZ data
*/
- template <typename PointT> inline void
+ template <typename PointT>
+ PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorMedianDistance::setInputCloud is deprecated. Please use setInputSource instead")
+ inline void
setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
{
- PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
}
/** \brief Provide a target point cloud dataset (must contain XYZ
{
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
}
/** \brief See if this rejector requires source points */
setSearchMethodTarget (const typename pcl::search::KdTree<PointT>::Ptr &tree,
bool force_no_recompute = false)
{
-
boost::static_pointer_cast< DataContainer<PointT> >
+ static_pointer_cast< DataContainer<PointT> >
(data_container_)->setSearchMethodTarget (tree, force_no_recompute );
}
#pragma once
#include <pcl/registration/correspondence_rejection.h>
-
+#include <pcl/memory.h> // for static_pointer_cast
namespace pcl
{
{
if (!data_container_)
data_container_.reset (new pcl::registration::DataContainer<PointT>);
-
boost::static_pointer_cast<pcl::registration::DataContainer<PointT> > (data_container_)->setInputSource (cloud);
+ static_pointer_cast<pcl::registration::DataContainer<PointT> > (data_container_)->setInputSource (cloud);
}
template <typename PointT> inline void
{
if (!data_container_)
data_container_.reset (new pcl::registration::DataContainer<PointT>);
-
boost::static_pointer_cast<pcl::registration::DataContainer<PointT> > (data_container_)->setInputTarget (cloud);
+ static_pointer_cast<pcl::registration::DataContainer<PointT> > (data_container_)->setInputTarget (cloud);
}
/** \brief See if this rejector requires source points */
/** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
* \param[in] cloud a cloud containing XYZ data
*/
+ PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorPoly::setInputCloud is deprecated. Please use setInputSource instead")
inline void
setInputCloud (const PointCloudSourceConstPtr &cloud)
{
- PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n",
- getClassName ().c_str ());
input_ = cloud;
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/correspondence_rejection.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#pragma once
#include <pcl/registration/correspondence_rejection.h>
+#include <pcl/memory.h> // for static_pointer_cast
#include <pcl/point_cloud.h>
+
namespace pcl
{
namespace registration
/** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
* \param[in] input a cloud containing XYZ data
*/
- template <typename PointT> inline void
+ template <typename PointT>
+ PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorSurfaceNormal::setInputCloud is deprecated. Please use setInputSource instead")
+ inline void
setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &input)
{
- PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
if (!data_container_)
{
PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
}
/** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
}
/** \brief Get the target input point cloud */
PCL_ERROR ("[pcl::registration::%s::getInputSource] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
- return (
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputSource ());
+ return (static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputSource ());
}
/** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
PCL_ERROR ("[pcl::registration::%s::setInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
}
/** \brief Provide a pointer to the search object used to find correspondences in
setSearchMethodTarget (const typename pcl::search::KdTree<PointT>::Ptr &tree,
bool force_no_recompute = false)
{
-
boost::static_pointer_cast< DataContainer<PointT> >
+ static_pointer_cast< DataContainer<PointT> >
(data_container_)->setSearchMethodTarget (tree, force_no_recompute );
}
PCL_ERROR ("[pcl::registration::%s::getInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
- return (
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputTarget ());
+ return (static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputTarget ());
}
/** \brief Set the normals computed on the input point cloud
PCL_ERROR ("[pcl::registration::%s::setInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
-
boost::static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setInputNormals (normals);
+ static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setInputNormals (normals);
}
/** \brief Get the normals computed on the input point cloud */
PCL_ERROR ("[pcl::registration::%s::getInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
- return (
boost::static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getInputNormals ());
+ return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getInputNormals ());
}
/** \brief Set the normals computed on the target point cloud
PCL_ERROR ("[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
-
boost::static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setTargetNormals (normals);
+ static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setTargetNormals (normals);
}
/** \brief Get the normals computed on the target point cloud */
PCL_ERROR ("[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
return;
}
- return (
boost::static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getTargetNormals ());
+ return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getTargetNormals ());
}
/** \brief Get the maximum distance used for thresholding in correspondence rejection. */
inline float
- getOverlapRatio () { return overlap_ratio_; };
+ getOverlapRatio () const { return overlap_ratio_; };
/** \brief Set a minimum number of correspondences. If the specified overlap ratio causes to have
* less correspondences, \a CorrespondenceRejectorTrimmed will try to return at least
/** \brief Get the minimum number of correspondences. */
inline unsigned int
- getMinCorrespondences () { return nr_min_correspondences_; };
+ getMinCorrespondences () const { return nr_min_correspondences_; };
/** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
#pragma once
#include <pcl/registration/correspondence_rejection.h>
+#include <pcl/memory.h> // for static_pointer_cast
#include <pcl/point_cloud.h>
#include <vector>
{
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
}
/** \brief Provide a source point cloud dataset (must contain XYZ
* data!), used to compute the correspondence distance.
* \param[in] cloud a cloud containing XYZ data
*/
- template <typename PointT> inline void
+ template <typename PointT>
+ PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorVarTrimmed::setInputCloud is deprecated. Please use setInputSource instead")
+ inline void
setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
{
- PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
}
/** \brief Provide a target point cloud dataset (must contain XYZ
{
if (!data_container_)
data_container_.reset (new DataContainer<PointT>);
-
boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
+ static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
}
setSearchMethodTarget (const typename pcl::search::KdTree<PointT>::Ptr &tree,
bool force_no_recompute = false)
{
-
boost::static_pointer_cast< DataContainer<PointT> >
+ static_pointer_cast< DataContainer<PointT> >
(data_container_)->setSearchMethodTarget (tree, force_no_recompute );
}
/** \brief finds the optimal inlier ratio. This is based on the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. Philips et al'
*/
- inline float optimizeInlierRatio (std::vector <double> &dists);
+ inline float optimizeInlierRatio (std::vector <double> &dists) const;
};
}
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/eigen.h>
#include <pcl/correspondence.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
namespace pcl
{
- /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the
- * generalized iterative closest point algorithm as described by Alex Segal et al. in
+ /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the
+ * generalized iterative closest point algorithm as described by Alex Segal et al. in
* http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf
- * The approach is based on using anistropic cost functions to optimize the alignment
+ * The approach is based on using anistropic cost functions to optimize the alignment
* after closest point assignments have been made.
- * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and
+ * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and
* FLANN.
* \author Nizar Sallem
* \ingroup registration
using MatricesVector = std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >;
using MatricesVectorPtr = shared_ptr<MatricesVector>;
using MatricesVectorConstPtr = shared_ptr<const MatricesVector>;
-
+
using InputKdTree = typename Registration<PointSource, PointTarget>::KdTree;
using InputKdTreePtr = typename Registration<PointSource, PointTarget>::KdTreePtr;
using Vector6d = Eigen::Matrix<double, 6, 1>;
/** \brief Empty constructor. */
- GeneralizedIterativeClosestPoint ()
+ GeneralizedIterativeClosestPoint ()
: k_correspondences_(20)
, gicp_epsilon_(0.001)
, rotation_epsilon_(2e-3)
, mahalanobis_(0)
, max_inner_iterations_(20)
+ ,translation_gradient_tolerance_(1e-2)
+ ,rotation_gradient_tolerance_(1e-2)
{
min_number_correspondences_ = 4;
reg_name_ = "GeneralizedIterativeClosestPoint";
// Set all the point.data[3] values to 1 to aid the rigid transformation
for (std::size_t i = 0; i < input.size (); ++i)
input[i].data[3] = 1.0;
-
+
pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputSource (cloud);
input_covariances_.reset ();
}
- /** \brief Provide a pointer to the covariances of the input source (if computed externally!).
+ /** \brief Provide a pointer to the covariances of the input source (if computed externally!).
* If not set, GeneralizedIterativeClosestPoint will compute the covariances itself.
* Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).
- * \param[in] target the input point cloud target
+ * \param[in] covariances the input source covariances
*/
- inline void
+ inline void
setSourceCovariances (const MatricesVectorPtr& covariances)
{
input_covariances_ = covariances;
}
-
+
/** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
* \param[in] target the input point cloud target
*/
- inline void
+ inline void
setInputTarget (const PointCloudTargetConstPtr &target) override
{
pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputTarget(target);
target_covariances_.reset ();
}
- /** \brief Provide a pointer to the covariances of the input target (if computed externally!).
+ /** \brief Provide a pointer to the covariances of the input target (if computed externally!).
* If not set, GeneralizedIterativeClosestPoint will compute the covariances itself.
* Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).
- * \param[in] target the input point cloud target
+ * \param[in] covariances the input target covariances
*/
- inline void
+ inline void
setTargetCovariances (const MatricesVectorPtr& covariances)
{
target_covariances_ = covariances;
}
-
+
/** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative
* non-linear Levenberg-Marquardt approach.
* \param[in] cloud_src the source point cloud dataset
const PointCloudTarget &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::Matrix4f &transformation_matrix);
-
+
/** \brief \return Mahalanobis distance matrix for the given point index */
inline const Eigen::Matrix3d& mahalanobis(std::size_t index) const
{
void
computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const;
- /** \brief Set the rotation epsilon (maximum allowable difference between two
- * consecutive rotations) in order for an optimization to be considered as having
+ /** \brief Set the rotation epsilon (maximum allowable difference between two
+ * consecutive rotations) in order for an optimization to be considered as having
* converged to the final solution.
* \param epsilon the rotation epsilon
*/
- inline void
+ inline void
setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; }
- /** \brief Get the rotation epsilon (maximum allowable difference between two
+ /** \brief Get the rotation epsilon (maximum allowable difference between two
* consecutive rotations) as set by the user.
*/
- inline double
- getRotationEpsilon () { return (rotation_epsilon_); }
+ inline double
+ getRotationEpsilon () const { return rotation_epsilon_; }
/** \brief Set the number of neighbors used when selecting a point neighbourhood
- * to compute covariances.
- * A higher value will bring more accurate covariance matrix but will make
+ * to compute covariances.
+ * A higher value will bring more accurate covariance matrix but will make
* covariances computation slower.
* \param k the number of neighbors to use when computing covariances
*/
void
setCorrespondenceRandomness (int k) { k_correspondences_ = k; }
- /** \brief Get the number of neighbors used when computing covariances as set by
- * the user
+ /** \brief Get the number of neighbors used when computing covariances as set by
+ * the user
*/
int
- getCorrespondenceRandomness () { return (k_correspondences_); }
+ getCorrespondenceRandomness () const { return k_correspondences_; }
- /** set maximum number of iterations at the optimization step
+ /** \brief Set maximum number of iterations at the optimization step
* \param[in] max maximum number of iterations for the optimizer
*/
void
setMaximumOptimizerIterations (int max) { max_inner_iterations_ = max; }
- ///\return maximum number of iterations at the optimization step
+ /** \brief Return maximum number of iterations at the optimization step
+ */
int
- getMaximumOptimizerIterations () { return (max_inner_iterations_); }
+ getMaximumOptimizerIterations () const { return max_inner_iterations_; }
+
+ /** \brief Set the minimal translation gradient threshold for early optimization stop
+ * \param[in] translation gradient threshold in meters
+ */
+ void
+ setTranslationGradientTolerance (double tolerance) { translation_gradient_tolerance_ = tolerance; }
+
+ /** \brief Return the minimal translation gradient threshold for early optimization stop
+ */
+ double
+ getTranslationGradientTolerance () const { return translation_gradient_tolerance_; }
+
+ /** \brief Set the minimal rotation gradient threshold for early optimization stop
+ * \param[in] rotation gradient threshold in radians
+ */
+ void
+ setRotationGradientTolerance (double tolerance) { rotation_gradient_tolerance_ = tolerance; }
+
+ /** \brief Return the minimal rotation gradient threshold for early optimization stop
+ */
+ double
+ getRotationGradientTolerance () const { return rotation_gradient_tolerance_; }
protected:
- /** \brief The number of neighbors used for covariances computation.
+ /** \brief The number of neighbors used for covariances computation.
* default: 20
*/
int k_correspondences_;
- /** \brief The epsilon constant for gicp paper; this is NOT the convergence
- * tolerance
+ /** \brief The epsilon constant for gicp paper; this is NOT the convergence
+ * tolerance
* default: 0.001
*/
double gicp_epsilon_;
- /** The epsilon constant for rotation error. (In GICP the transformation epsilon
+ /** The epsilon constant for rotation error. (In GICP the transformation epsilon
* is split in rotation part and translation part).
* default: 2e-3
*/
/** \brief Temporary pointer to the target dataset indices. */
const std::vector<int> *tmp_idx_tgt_;
-
/** \brief Input cloud points covariances. */
MatricesVectorPtr input_covariances_;
/** \brief Mahalanobis matrices holder. */
std::vector<Eigen::Matrix3d> mahalanobis_;
-
+
/** \brief maximum number of optimizations */
int max_inner_iterations_;
- /** \brief compute points covariances matrices according to the K nearest
+ /** \brief minimal translation gradient for early optimization stop */
+ double translation_gradient_tolerance_;
+
+ /** \brief minimal rotation gradient for early optimization stop */
+ double rotation_gradient_tolerance_;
+
+ /** \brief compute points covariances matrices according to the K nearest
* neighbors. K is set via setCorrespondenceRandomness() method.
* \param cloud pointer to point cloud
* \param tree KD tree performer for nearest neighbors search
* \param[out] cloud_covariances covariances matrices for each point in the cloud
*/
template<typename PointT>
- void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
+ void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
const typename pcl::search::KdTree<PointT>::Ptr tree,
MatricesVector& cloud_covariances);
- /** \return trace of mat1^t . mat2
+ /** \return trace of mat1^t . mat2
* \param mat1 matrix of dimension nxm
* \param mat2 matrix of dimension nxp
*/
- inline double
+ inline double
matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
{
double r = 0.;
* \param output the transformed input point cloud dataset using the rigid transformation found
* \param guess the initial guess of the transformation to compute
*/
- void
+ void
computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override;
/** \brief Search for the closest nearest neighbor of a given point.
* \param index vector of size 1 to store the index of the nearest neighbour found
* \param distance vector of size 1 to store the distance to nearest neighbour found
*/
- inline bool
+ inline bool
searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance)
{
int k = tree_->nearestKSearch (query, 1, index, distance);
/// \brief compute transformation matrix from transformation matrix
void applyState(Eigen::Matrix4f &t, const Vector6d& x) const;
-
+
/// \brief optimization functor structure
struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double,6>
{
double operator() (const Vector6d& x) override;
void df(const Vector6d &x, Vector6d &df) override;
void fdf(const Vector6d &x, double &f, Vector6d &df) override;
+ BFGSSpace::Status checkGradient(const Vector6d& g) override;
const GeneralizedIterativeClosestPoint *gicp_;
};
-
+
std::function<void(const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &src_indices,
const pcl::PointCloud<PointTarget> &cloud_tgt,
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/transformation_estimation_svd.h>
#pragma once
// PCL includes
-#include <pcl/make_shared.h>
+#include <pcl/memory.h> // for dynamic_pointer_cast, pcl::make_shared, shared_ptr
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/default_convergence_criteria.h>
+
namespace pcl
{
/** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
{
enforce_same_direction_normals_ = enforce_same_direction_normals;
- auto symmetric_transformation_estimation =
boost::dynamic_pointer_cast<pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar> >(transformation_estimation_);
+ auto symmetric_transformation_estimation = dynamic_pointer_cast<pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar> >(transformation_estimation_);
if (symmetric_transformation_estimation)
symmetric_transformation_estimation->setEnforceSameDirectionNormals (enforce_same_direction_normals_);
}
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
#include <pcl/common/io.h>
#include <pcl/common/copy_point.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::setInputTarget (
+CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::setInputTarget (
const PointCloudTargetConstPtr &cloud)
{
if (cloud->points.empty ())
target_cloud_updated_ = true;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> bool
-
pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ()
+CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ()
{
if (!target_)
{
return (PCLBase<PointSource>::initCompute ());
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> bool
-
pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal ()
+CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal ()
{
// Only update source kd-tree if a new target cloud was set
if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
return (true);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences (
+CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences (
pcl::Correspondences &correspondences, double max_distance)
{
if (!initCompute ())
std::vector<float> distance (1);
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
-
+
// Check if the template types are the same. If true, avoid a copy.
// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
if (isSamePointType<PointSource, PointTarget> ())
else
{
PointTarget pt;
-
+
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{
deinitCompute ();
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (
+CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (
pcl::Correspondences &correspondences, double max_distance)
{
if (!initCompute ())
{
PointTarget pt_src;
PointSource pt_tgt;
-
+
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{
deinitCompute ();
}
+} // namespace registration
+} // namespace pcl
+
//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS pcl::registration::CorrespondenceEstimation<T,U>;
#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
#include <pcl/common/copy_point.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
-
pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::initCompute ()
{
if (!source_normals_ || !target_normals_)
{
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-
pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences (
+CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences (
pcl::Correspondences &correspondences, double max_distance)
{
if (!initCompute ())
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
- float min_dist = std::numeric_limits<float>::max ();
int min_index = 0;
-
+
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
// Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
- min_dist = std::numeric_limits<float>::max ();
-
+ float min_dist = std::numeric_limits<float>::max ();
+
// Find the best correspondence
for (std::size_t j = 0; j < nn_indices.size (); j++)
{
source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
-
+
if (dist < min_dist)
{
min_dist = dist;
else
{
PointTarget pt;
-
+
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
{
tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
-
+
// Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
- min_dist = std::numeric_limits<float>::max ();
-
+ float min_dist = std::numeric_limits<float>::max ();
+
// Find the best correspondence
for (std::size_t j = 0; j < nn_indices.size (); j++)
{
source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
-
+
if (dist < min_dist)
{
min_dist = dist;
}
if (min_dist > max_distance)
continue;
-
+
corr.index_query = *idx_i;
corr.index_match = nn_indices[min_index];
corr.distance = nn_dists[min_index];//min_dist;
deinitCompute ();
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-
pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences (
+CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences (
pcl::Correspondences &correspondences, double max_distance)
{
if (!initCompute ())
std::vector<int> index_reciprocal (1);
std::vector<float> distance_reciprocal (1);
- float min_dist = std::numeric_limits<float>::max ();
int min_index = 0;
-
+
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
int target_idx = 0;
tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
// Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
- min_dist = std::numeric_limits<float>::max ();
-
+ float min_dist = std::numeric_limits<float>::max ();
+
// Find the best correspondence
for (std::size_t j = 0; j < nn_indices.size (); j++)
{
source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
-
+
if (dist < min_dist)
{
min_dist = dist;
else
{
PointTarget pt;
-
+
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
{
tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
-
+
// Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
- min_dist = std::numeric_limits<float>::max ();
-
+ float min_dist = std::numeric_limits<float>::max ();
+
// Find the best correspondence
for (std::size_t j = 0; j < nn_indices.size (); j++)
{
source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
-
+
if (dist < min_dist)
{
min_dist = dist;
}
if (min_dist > max_distance)
continue;
-
+
// Check if the correspondence is reciprocal
target_idx = nn_indices[min_index];
tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
deinitCompute ();
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
#include <pcl/common/copy_point.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
-
pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::initCompute ()
{
if (!source_normals_)
{
return (CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ());
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-
pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences (
+CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences (
pcl::Correspondences &correspondences, double max_distance)
{
if (!initCompute ())
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
- double min_dist = std::numeric_limits<double>::max ();
int min_index = 0;
-
+
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
// Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
- min_dist = std::numeric_limits<double>::max ();
-
+ double min_dist = std::numeric_limits<double>::max ();
+
// Find the best correspondence
for (std::size_t j = 0; j < nn_indices.size (); j++)
{
- // computing the distance between a point and a line in 3d.
+ // computing the distance between a point and a line in 3d.
// Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
Eigen::Vector3d V (pt.x, pt.y, pt.z);
Eigen::Vector3d C = N.cross (V);
-
+
// Check if we have a better correspondence
double dist = C.dot (C);
if (dist < min_dist)
else
{
PointTarget pt;
-
+
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
{
tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
-
+
// Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
- min_dist = std::numeric_limits<double>::max ();
-
+ double min_dist = std::numeric_limits<double>::max ();
+
// Find the best correspondence
for (std::size_t j = 0; j < nn_indices.size (); j++)
{
pt.x = target_->points[nn_indices[j]].x - pt_src.x;
pt.y = target_->points[nn_indices[j]].y - pt_src.y;
pt.z = target_->points[nn_indices[j]].z - pt_src.z;
-
+
const NormalT &normal = source_normals_->points[*idx_i];
Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
Eigen::Vector3d V (pt.x, pt.y, pt.z);
Eigen::Vector3d C = N.cross (V);
-
+
// Check if we have a better correspondence
double dist = C.dot (C);
if (dist < min_dist)
}
if (min_dist > max_distance)
continue;
-
+
corr.index_query = *idx_i;
corr.index_match = nn_indices[min_index];
corr.distance = nn_dists[min_index];//min_dist;
deinitCompute ();
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-
pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences (
+CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences (
pcl::Correspondences &correspondences, double max_distance)
{
if (!initCompute ())
std::vector<int> index_reciprocal (1);
std::vector<float> distance_reciprocal (1);
- double min_dist = std::numeric_limits<double>::max ();
int min_index = 0;
-
+
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
int target_idx = 0;
tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
// Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
- min_dist = std::numeric_limits<double>::max ();
-
+ double min_dist = std::numeric_limits<double>::max ();
+
// Find the best correspondence
for (std::size_t j = 0; j < nn_indices.size (); j++)
{
- // computing the distance between a point and a line in 3d.
+ // computing the distance between a point and a line in 3d.
// Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
Eigen::Vector3d V (pt.x, pt.y, pt.z);
Eigen::Vector3d C = N.cross (V);
-
+
// Check if we have a better correspondence
double dist = C.dot (C);
if (dist < min_dist)
else
{
PointTarget pt;
-
+
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
{
tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
// Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
- min_dist = std::numeric_limits<double>::max ();
-
+ double min_dist = std::numeric_limits<double>::max ();
+
// Find the best correspondence
for (std::size_t j = 0; j < nn_indices.size (); j++)
{
// Copy the source data to a target PointTarget format so we can search in the tree
copyPoint (input_->points[*idx_i], pt_src);
- // computing the distance between a point and a line in 3d.
+ // computing the distance between a point and a line in 3d.
// Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
pt.x = target_->points[nn_indices[j]].x - pt_src.x;
pt.y = target_->points[nn_indices[j]].y - pt_src.y;
pt.z = target_->points[nn_indices[j]].z - pt_src.z;
-
+
const NormalT &normal = source_normals_->points[*idx_i];
Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
Eigen::Vector3d V (pt.x, pt.y, pt.z);
Eigen::Vector3d C = N.cross (V);
-
+
// Check if we have a better correspondence
double dist = C.dot (C);
if (dist < min_dist)
deinitCompute ();
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
*
*/
+
#ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
#define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> bool
-
pcl::registration::CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::initCompute ()
+CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::initCompute ()
{
// Set the target_cloud_updated_ variable to true, so that the kd-tree is not built - it is not needed for this class
target_cloud_updated_ = false;
return (true);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::determineCorrespondences (
+CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::determineCorrespondences (
pcl::Correspondences &correspondences,
double max_distance)
{
correspondences.resize (c_index);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (
+CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (
pcl::Correspondences &correspondences,
double max_distance)
{
determineCorrespondences (correspondences, max_distance);
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
* $Id$
*
*/
+
+
#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename FeatureT> inline void
-pcl::registration::CorrespondenceRejectorFeatures::setSourceFeature (
+
+namespace pcl
+{
+
+namespace registration
+{
+
+template <typename FeatureT> inline void
+CorrespondenceRejectorFeatures::setSourceFeature (
const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature, const std::string &key)
{
if (features_map_.count (key) == 0)
boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setSourceFeature (source_feature);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
-pcl::registration::CorrespondenceRejectorFeatures::getSourceFeature (const std::string &key)
+
+template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
+CorrespondenceRejectorFeatures::getSourceFeature (const std::string &key)
{
if (features_map_.count (key) == 0)
return (nullptr);
return (boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->getSourceFeature ());
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename FeatureT> inline void
-pcl::registration::CorrespondenceRejectorFeatures::setTargetFeature (
+
+template <typename FeatureT> inline void
+CorrespondenceRejectorFeatures::setTargetFeature (
const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature, const std::string &key)
{
if (features_map_.count (key) == 0)
boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setTargetFeature (target_feature);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
-pcl::registration::CorrespondenceRejectorFeatures::getTargetFeature (const std::string &key)
+
+template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
+CorrespondenceRejectorFeatures::getTargetFeature (const std::string &key)
{
if (features_map_.count (key) == 0)
return (nullptr);
return (boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->getTargetFeature ());
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename FeatureT> inline void
-pcl::registration::CorrespondenceRejectorFeatures::setDistanceThreshold (
+
+template <typename FeatureT> inline void
+CorrespondenceRejectorFeatures::setDistanceThreshold (
double thresh, const std::string &key)
{
if (features_map_.count (key) == 0)
}
-
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename FeatureT> inline void
-pcl::registration::CorrespondenceRejectorFeatures::setFeatureRepresentation (
+template <typename FeatureT> inline void
+CorrespondenceRejectorFeatures::setFeatureRepresentation (
const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
const std::string &key)
{
boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setFeatureRepresentation (fr);
}
+} // namespace registration
+} // namespace pcl
#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_ */
+
* POSSIBILITY OF SUCH DAMAGE.
*
*/
+
+
#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
-///////////////////////////////////////////////////////////////////////////////////////////
-template <typename SourceT, typename TargetT> void
-pcl::registration::CorrespondenceRejectorPoly<SourceT, TargetT>::getRemainingCorrespondences (
- const pcl::Correspondences& original_correspondences,
+
+namespace pcl
+{
+
+namespace registration
+{
+
+template <typename SourceT, typename TargetT> void
+CorrespondenceRejectorPoly<SourceT, TargetT>::getRemainingCorrespondences (
+ const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
// This is reset after all the checks below
remaining_correspondences = original_correspondences;
-
+
// Check source/target
if (!input_)
{
getClassName ().c_str ());
return;
}
-
+
// Check cardinality
if (cardinality_ < 2)
{
getClassName ().c_str() );
return;
}
-
+
// Number of input correspondences
const int nr_correspondences = static_cast<int> (original_correspondences.size ());
getClassName ().c_str() );
return;
}
-
+
// Check similarity
if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f)
{
getClassName ().c_str() );
return;
}
-
+
// Similarity, squared
similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_;
// Initialization of result
remaining_correspondences.clear ();
remaining_correspondences.reserve (nr_correspondences);
-
+
// Number of times a correspondence is sampled and number of times it was accepted
std::vector<int> num_samples (nr_correspondences, 0);
std::vector<int> num_accepted (nr_correspondences, 0);
-
+
// Main loop
for (int i = 0; i < iterations_; ++i)
{
// Sample cardinality_ correspondences without replacement
const std::vector<int> idx = getUniqueRandomIndices (nr_correspondences, cardinality_);
-
+
// Verify the polygon similarity
if (thresholdPolygon (original_correspondences, idx))
{
++num_samples[ idx[j] ];
}
}
-
+
// Now calculate the acceptance rate of each correspondence
std::vector<float> accept_rate (nr_correspondences, 0.0f);
for (int i = 0; i < nr_correspondences; ++i)
else
accept_rate[i] = static_cast<float> (num_accepted[i]) / static_cast<float> (numsi);
}
-
+
// Compute a histogram in range [0,1] for acceptance rates
const int hist_size = nr_correspondences / 2; // TODO: Optimize this
const std::vector<int> histogram = computeHistogram (accept_rate, 0.0f, 1.0f, hist_size);
-
+
// Find the cut point between outliers and inliers using Otsu's thresholding method
const int cut_idx = findThresholdOtsu (histogram);
const float cut = static_cast<float> (cut_idx) / static_cast<float> (hist_size);
-
+
// Threshold
for (int i = 0; i < nr_correspondences; ++i)
if (accept_rate[i] > cut)
remaining_correspondences.push_back (original_correspondences[i]);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename SourceT, typename TargetT> std::vector<int>
-pcl::registration::CorrespondenceRejectorPoly<SourceT, TargetT>::computeHistogram (const std::vector<float>& data,
- float lower, float upper, int bins)
+
+template <typename SourceT, typename TargetT> std::vector<int>
+CorrespondenceRejectorPoly<SourceT, TargetT>::computeHistogram (const std::vector<float>& data,
+ float lower, float upper, int bins)
{
// Result
std::vector<int> result (bins, 0);
-
+
// Last index into result and increment factor from data value --> index
const int last_idx = bins - 1;
const float idx_per_val = static_cast<float> (bins) / (upper - lower);
-
+
// Accumulate
for (const float &value : data)
++result[ std::min (last_idx, int (value*idx_per_val)) ];
-
+
return (result);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename SourceT, typename TargetT> int
-pcl::registration::CorrespondenceRejectorPoly<SourceT, TargetT>::findThresholdOtsu (const std::vector<int>& histogram)
+
+template <typename SourceT, typename TargetT> int
+CorrespondenceRejectorPoly<SourceT, TargetT>::findThresholdOtsu (const std::vector<int>& histogram)
{
// Precision
const double eps = std::numeric_limits<double>::epsilon();
-
+
// Histogram dimension
const int nbins = static_cast<int> (histogram.size ());
-
+
// Mean and inverse of the number of data points
double mean = 0.0;
double sum_inv = 0.0;
}
sum_inv = 1.0/sum_inv;
mean *= sum_inv;
-
+
// Probability and mean of class 1 (data to the left of threshold)
double class_mean1 = 0.0;
double class_prob1 = 0.0;
double class_prob2 = 1.0;
-
+
// Maximized between class variance and associated bin value
double between_class_variance_max = 0.0;
int result = 0;
-
+
// Loop over all bin values
for (int i = 0; i < nbins; ++i)
{
class_mean1 *= class_prob1;
-
+
// Probability of bin i
const double prob_i = static_cast<double> (histogram[i]) * sum_inv;
-
+
// Class probability 1: sum of probabilities from 0 to i
class_prob1 += prob_i;
-
+
// Class probability 2: sum of probabilities from i+1 to nbins-1
class_prob2 -= prob_i;
-
+
// Avoid division by zero below
if (std::min (class_prob1,class_prob2) < eps || std::max (class_prob1,class_prob2) > 1.0-eps)
continue;
-
+
// Class mean 1: sum of probabilities from 0 to i, weighted by bin value
class_mean1 = (class_mean1 + static_cast<double> (i) * prob_i) / class_prob1;
-
+
// Class mean 2: sum of probabilities from i+1 to nbins-1, weighted by bin value
const double class_mean2 = (mean - class_prob1*class_mean1) / class_prob2;
-
+
// Between class variance
const double between_class_variance = class_prob1 * class_prob2
* (class_mean1 - class_mean2)
* (class_mean1 - class_mean2);
-
+
// If between class variance is maximized, update result
if (between_class_variance > between_class_variance_max)
{
result = i;
}
}
-
+
return (result);
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
#include <unordered_map>
-///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences (
- const pcl::Correspondences& original_correspondences,
+
+namespace pcl
+{
+
+namespace registration
+{
+
+template <typename PointT> void
+CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences (
+ const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
if (!input_)
}
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
+
*
*
*/
+
#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_
#include <unordered_map>
-///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::registration::CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences (
- const pcl::Correspondences& original_correspondences,
+
+namespace pcl
+{
+
+namespace registration
+{
+
+template <typename PointT> void
+CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences (
+ const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
if (!input_)
best_transformation_.row (3) = model_coefficients.segment<4>(12);
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_
#include <cstddef>
#include <vector>
+
+namespace pcl
+{
+
+namespace registration
+{
+
inline void
-pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev)
+getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev)
{
if (correspondences.empty ())
return;
}
inline void
-pcl::registration::getQueryIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
+getQueryIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
{
indices.resize (correspondences.size ());
for (std::size_t i = 0; i < correspondences.size (); ++i)
inline void
-pcl::registration::getMatchIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
+getMatchIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
{
indices.resize (correspondences.size ());
for (std::size_t i = 0; i < correspondences.size (); ++i)
indices[i] = correspondences[i].index_match;
}
+
+} // namespace registration
+} // namespace pcl
+
#include <pcl/console/print.h>
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename Scalar> bool
-pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged ()
+DefaultConvergenceCriteria<Scalar>::hasConverged ()
{
if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED)
{
iterations_similar_transforms_ = 0;
convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED;
}
-
+
bool is_similar = false;
PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n", iterations_, max_iterations_);
}
is_similar = true;
}
-
+
// Relative
if (std::abs (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_)
{
return (false);
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
#define PCL_REGISTRATION_IMPL_GICP_HPP_
#include <pcl/registration/boost.h>
#include <pcl/registration/exceptions.h>
-////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointSource, typename PointTarget>
template<typename PointT> void
-
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
-
const typename pcl::search::KdTree<PointT>::Ptr kdtree,
- MatricesVector& cloud_covariances)
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
+ const typename pcl::search::KdTree<PointT>::Ptr kdtree,
+ MatricesVector& cloud_covariances)
{
if (k_correspondences_ > int (cloud->size ()))
{
}
}
-////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget> void
-
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const
{
Eigen::Matrix3d dR_dPhi;
Eigen::Matrix3d dR_dTheta;
g[5] = matricesInnerProd(dR_dPsi, R);
}
-////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget> void
-
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
- const std::vector<int> &indices_src,
- const PointCloudTarget &cloud_tgt,
- const std::vector<int> &indices_tgt,
- Eigen::Matrix4f &transformation_matrix)
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
+ const std::vector<int> &indices_src,
+ const PointCloudTarget &cloud_tgt,
+ const std::vector<int> &indices_tgt,
+ Eigen::Matrix4f &transformation_matrix)
{
if (indices_src.size () < 4) // need at least 4 samples
{
}
// Set the initial solution
Vector6d x = Vector6d::Zero ();
+ // translation part
x[0] = transformation_matrix (0,3);
x[1] = transformation_matrix (1,3);
x[2] = transformation_matrix (2,3);
+ // rotation part (Z Y X euler angles convention)
+ // see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
x[3] = std::atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
x[4] = asin (-transformation_matrix (2,0));
x[5] = std::atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
tmp_idx_tgt_ = &indices_tgt;
// Optimize using forward-difference approximation LM
- const double gradient_tol = 1e-2;
OptimizationFunctorWithIndices functor(this);
BFGS<OptimizationFunctorWithIndices> bfgs (functor);
bfgs.parameters.sigma = 0.01;
{
break;
}
- result = bfgs.testGradient(gradient_tol);
+ result = bfgs.testGradient();
} while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
{
"[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
}
-////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget> inline double
-
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::operator() (const Vector6d& x)
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::operator() (const Vector6d& x)
{
Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
gicp_->applyState(transformation_matrix, x);
return f/m;
}
-////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget> inline void
-
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g)
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g)
{
Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
gicp_->applyState(transformation_matrix, x);
gicp_->computeRDerivative(x, R, g);
}
-////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget> inline void
-
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g)
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g)
{
Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
gicp_->applyState(transformation_matrix, x);
gicp_->computeRDerivative(x, R, g);
}
-////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> inline BFGSSpace::Status
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::checkGradient(const Vector6d& g)
+{
+ auto translation_epsilon = gicp_->translation_gradient_tolerance_;
+ auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
+
+ if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
+ return BFGSSpace::NegativeGradientEpsilon;
+
+ // express translation gradient as norm of translation parameters
+ auto translation_grad = g.head<3>().norm();
+
+ // express rotation gradient as a norm of rotation parameters
+ auto rotation_grad = g.tail<3>().norm();
+
+ if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
+ return BFGSSpace::Success;
+
+ return BFGSSpace::Running;
+}
+
template <typename PointSource, typename PointTarget> inline void
-
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
using namespace std;
// Compute target cloud covariance matrices
if ((!target_covariances_) || (target_covariances_->empty ()))
{
- target_covariances_.reset (new MatricesVector);
+ target_covariances_.reset (new MatricesVector);
computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
}
// Compute input cloud covariance matrices
pcl::transformPointCloud (*input_, output, final_transformation_);
}
+
template <typename PointSource, typename PointTarget> void
-
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(Eigen::Matrix4f &t, const Vector6d& x) const
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(Eigen::Matrix4f &t, const Vector6d& x) const
{
- // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
+ // Z Y X euler angles convention
Eigen::Matrix3f R;
R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
* Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
t.col (3) += T;
}
+} // namespace pcl
+
#endif //PCL_REGISTRATION_IMPL_GICP_HPP_
+
#include <pcl/registration/ia_fpcs.h>
#include <pcl/common/time.h>
#include <pcl/common/distances.h>
+#include <pcl/common/utils.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/registration/transformation_estimation_3point.h>
std::vector <int> ids (2);
std::vector <float> dists_sqr (2);
-#ifdef _OPENMP
+ pcl::utils::ignore(nr_threads);
#pragma omp parallel for \
- reduction (+:mean_dist, num) \
- private (ids, dists_sqr) shared (tree, cloud) firstprivate (s, max_dist_sqr) \
- default (none)num_threads (nr_threads)
-#endif
-
+ default(none) \
+ shared(tree, cloud) \
+ private(ids, dists_sqr) \
+ reduction(+:mean_dist, num) \
+ firstprivate(s, max_dist_sqr) \
+ num_threads(nr_threads)
for (int i = 0; i < 1000; i++)
{
tree.nearestKSearch (cloud->points[rand () % s], 2, ids, dists_sqr);
std::vector <int> ids (2);
std::vector <float> dists_sqr (2);
-#ifdef _OPENMP
+ pcl::utils::ignore(nr_threads);
#pragma omp parallel for \
- reduction (+:mean_dist, num) \
- private (ids, dists_sqr) shared (tree, cloud, indices) firstprivate (s, max_dist_sqr) \
- default (none)num_threads (nr_threads)
-#endif
-
+ default(none) \
+ shared(tree, cloud, indices) \
+ private(ids, dists_sqr) \
+ reduction(+:mean_dist, num) \
+ firstprivate(s, max_dist_sqr) \
+ num_threads(nr_threads)
for (int i = 0; i < 1000; i++)
{
tree.nearestKSearch (cloud->points[indices[rand () % s]], 2, ids, dists_sqr);
std::vector <MatchingCandidates> all_candidates (max_iterations_);
pcl::StopWatch timer;
- #ifdef _OPENMP
- #pragma omp parallel num_threads (nr_threads_)
- #endif
+ #pragma omp parallel \
+ default(none) \
+ shared(abort, all_candidates, timer) \
+ num_threads(nr_threads_)
{
#ifdef _OPENMP
std::srand (static_cast <unsigned int> (std::time (NULL)) ^ omp_get_thread_num ());
- #pragma omp for schedule (dynamic)
+ #pragma omp for schedule(dynamic)
#endif
for (int i = 0; i < max_iterations_; i++)
{
-
- #ifdef _OPENMP
#pragma omp flush (abort)
- #endif
MatchingCandidates candidates (1);
std::vector <int> base_indices (4);
abort = (abort ? abort : timer.getTimeSeconds () > max_runtime_);
- #ifdef _OPENMP
#pragma omp flush (abort)
- #endif
}
}
}
#ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
#define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
-
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::KFPCSInitialAlignment () :
+KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::KFPCSInitialAlignment () :
lower_trl_boundary_ (-1.f),
- upper_trl_boundary_ (-1.f),
+ upper_trl_boundary_ (-1.f),
lambda_ (0.5f),
use_trl_score_ (false),
indices_validation_ (new std::vector <int>)
}
-///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
-
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ()
{
// due to sparse keypoint cloud, do not normalize delta with estimated point density
if (normalize_delta_)
}
-///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::handleMatches (
+KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::handleMatches (
const std::vector <int> &base_indices,
std::vector <std::vector <int> > &matches,
MatchingCandidates &candidates)
{
candidates.clear ();
- float fitness_score = FLT_MAX;
// loop over all Candidate matches
for (auto &match : matches)
{
Eigen::Matrix4f transformation_temp;
pcl::Correspondences correspondences_temp;
- fitness_score = FLT_MAX; // reset to FLT_MAX to accept all candidates and not only best
+ float fitness_score = FLT_MAX; // reset to FLT_MAX to accept all candidates and not only best
// determine corresondences between base and match according to their distance to centroid
linkMatchWithBase (base_indices, match, correspondences_temp);
}
-///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateTransformation (
+KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateTransformation (
Eigen::Matrix4f &transformation,
float &fitness_score)
{
const std::size_t nr_points = source_transformed.size ();
float score_a = 0.f, score_b = 0.f;
-
+
// residual costs based on mse
std::vector <int> ids;
std::vector <float> dists_sqr;
}
-///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::finalCompute (
+KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::finalCompute (
const std::vector <MatchingCandidates > &candidates)
{
// reorganize candidates into single vector
converged_ = fitness_score_ < score_threshold_;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getNBestCandidates (
+KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getNBestCandidates (
int n,
float min_angle3d,
float min_translation3d,
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getTBestCandidates (
+KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getTBestCandidates (
float t,
float min_angle3d,
float min_translation3d,
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+} // namespace registration
+} // namespace pcl
#endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_
+
#include <pcl/common/distances.h>
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features)
+
+namespace pcl
+{
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features)
{
if (features == nullptr || features->empty ())
{
input_features_ = features;
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features)
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features)
{
if (features == nullptr || features->empty ())
{
feature_tree_->setInputCloud (target_features_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples (
- const PointCloudSource &cloud, int nr_samples, float min_sample_distance,
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples (
+ const PointCloudSource &cloud, int nr_samples, float min_sample_distance,
std::vector<int> &sample_indices)
{
if (nr_samples > static_cast<int> (cloud.points.size ()))
}
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
- const FeatureCloud &input_features, const std::vector<int> &sample_indices,
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
+ const FeatureCloud &input_features, const std::vector<int> &sample_indices,
std::vector<int> &corresponding_indices)
{
std::vector<int> nn_indices (k_correspondences_);
}
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> float
-
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric (
+
+template <typename PointSource, typename PointTarget, typename FeatureT> float
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric (
const PointCloudSource &cloud, float)
{
std::vector<int> nn_index (1);
return (error);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
// Some sanity checks first
if (!input_features_)
final_transformation_ = guess;
int i_iter = 0;
converged_ = false;
- if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
+ if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
{
// If guess is not the Identity matrix we check it.
transformPointCloud (*input_, input_transformed, final_transformation_);
transformPointCloud (*input_, output, final_transformation_);
}
+} // namespace pcl
+
#endif //#ifndef IA_RANSAC_HPP_
#include <pcl/registration/boost.h>
#include <pcl/correspondence.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud (
- const PointCloudSource &input,
- PointCloudSource &output,
+IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud (
+ const PointCloudSource &input,
+ PointCloudSource &output,
const Matrix4 &transform)
{
Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
}
}
-
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
+IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
PointCloudSource &output, const Matrix4 &guess)
{
// Point cloud containing the correspondences of each point in <input, indices>
}
else
*input_transformed = *input_;
-
+
transformation_ = Matrix4::Identity ();
// Make blobs if necessary
// Transform the data
transformCloud (*input_transformed, *input_transformed, transformation_);
- // Obtain the final transformation
+ // Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_;
transformCloud (*input_, output, final_transformation_);
}
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
+IterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
{
need_source_blob_ = false;
need_target_blob_ = false;
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>::transformCloud (
- const PointCloudSource &input,
- PointCloudSource &output,
+IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>::transformCloud (
+ const PointCloudSource &input,
+ PointCloudSource &output,
const Matrix4 &transform)
{
pcl::transformPointCloudWithNormals (input, output, transform);
}
+} // namespace pcl
#endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */
+
#ifndef PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_
#define PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointT, typename Scalar>
-
pcl::registration::IncrementalRegistration<PointT, Scalar>::IncrementalRegistration () :
+IncrementalRegistration<PointT, Scalar>::IncrementalRegistration () :
delta_transform_ (Matrix4::Identity ()),
abs_transform_ (Matrix4::Identity ())
{}
template <typename PointT, typename Scalar> bool
-
pcl::registration::IncrementalRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate)
+IncrementalRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate)
{
assert (registration_);
}
template <typename PointT, typename Scalar> inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
-
pcl::registration::IncrementalRegistration<PointT, Scalar>::getDeltaTransform () const
+IncrementalRegistration<PointT, Scalar>::getDeltaTransform () const
{
return (delta_transform_);
}
template <typename PointT, typename Scalar> inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
-
pcl::registration::IncrementalRegistration<PointT, Scalar>::getAbsoluteTransform () const
+IncrementalRegistration<PointT, Scalar>::getAbsoluteTransform () const
{
return (abs_transform_);
}
template <typename PointT, typename Scalar> inline void
-
pcl::registration::IncrementalRegistration<PointT, Scalar>::reset ()
+IncrementalRegistration<PointT, Scalar>::reset ()
{
last_cloud_.reset ();
delta_transform_ = abs_transform_ = Matrix4::Identity ();
}
template <typename PointT, typename Scalar> inline void
-
pcl::registration::IncrementalRegistration<PointT, Scalar>::setRegistration (RegistrationPtr registration)
+IncrementalRegistration<PointT, Scalar>::setRegistration (RegistrationPtr registration)
{
registration_ = registration;
}
+} // namespace registration
+} // namespace pcl
+
#endif /*PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_*/
+
/*
* Software License Agreement (BSD License)
- *
+ *
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2012, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
- *
+ *
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
- *
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#include <pcl/console/print.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+namespace pcl
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
+JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
PointCloudSource &output, const Matrix4 &guess)
{
// Point clouds containing the correspondences of each point in <input, indices>
correspondence_estimations_.resize (sources_.size ());
for (std::size_t i = 0; i < sources_.size (); i++)
{
- correspondence_estimations_[i] = correspondence_estimation_->clone ();
+ correspondence_estimations_[i] = correspondence_estimation_->clone ();
KdTreeReciprocalPtr src_tree (new KdTreeReciprocal);
KdTreePtr tgt_tree (new KdTree);
correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree);
target_offset += targets_[i]->size ();
}
-
-
transformation_ = Matrix4::Identity ();
// Make blobs if necessary
determineRequiredBlobData ();
toPCLPointCloud2 (*inputs_transformed[i], *input_transformed_blob);
correspondence_estimations_[i]->setSourceNormals (input_transformed_blob);
}
-
+
// Estimate correspondences on each cloud pair separately
if (use_reciprocal_correspondence_)
{
{
correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
}
- PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n",
- getClassName ().c_str (),
+ PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n",
+ getClassName ().c_str (),
partial_correspondences_[i]->size (), i);
for (std::size_t j = 0; j < partial_correspondences_[i]->size (); j++)
{
}
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ());
-
+
PCLPointCloud2::Ptr inputs_transformed_combined_blob;
if (need_source_blob_)
{
this->transformCloud (*inputs_transformed[i], *inputs_transformed[i], transformation_);
}
- // Obtain the final transformation
+ // Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_;
}
while (!converged_);
- PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
+ PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
}
- // By definition, this method will return an empty cloud (for compliance with the ICP API).
+ // By definition, this method will return an empty cloud (for compliance with the ICP API).
// We can figure out a better solution, if necessary.
output = PointCloudSource ();
}
- template <typename PointSource, typename PointTarget, typename Scalar> void
-pcl::JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
+
+template <typename PointSource, typename PointTarget, typename Scalar> void
+JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
{
need_source_blob_ = false;
need_target_blob_ = false;
}
}
+} // namespace pcl
#endif /* PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ */
-
#include <tuple>
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template<typename PointT> inline void
-
pcl::registration::LUM<PointT>::setLoopGraph (const SLAMGraphPtr &slam_graph)
+LUM<PointT>::setLoopGraph (const SLAMGraphPtr &slam_graph)
{
slam_graph_ = slam_graph;
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT> inline typename
pcl::registration::LUM<PointT>::SLAMGraphPtr
-
pcl::registration::LUM<PointT>::getLoopGraph () const
+
+template<typename PointT> inline typename LUM<PointT>::SLAMGraphPtr
+LUM<PointT>::getLoopGraph () const
{
return (slam_graph_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT> typename
pcl::registration::LUM<PointT>::SLAMGraph::vertices_size_type
-
pcl::registration::LUM<PointT>::getNumVertices () const
+
+template<typename PointT> typename LUM<PointT>::SLAMGraph::vertices_size_type
+LUM<PointT>::getNumVertices () const
{
return (num_vertices (*slam_graph_));
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> void
-
pcl::registration::LUM<PointT>::setMaxIterations (int max_iterations)
+LUM<PointT>::setMaxIterations (int max_iterations)
{
max_iterations_ = max_iterations;
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline int
-
pcl::registration::LUM<PointT>::getMaxIterations () const
+LUM<PointT>::getMaxIterations () const
{
return (max_iterations_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> void
-
pcl::registration::LUM<PointT>::setConvergenceThreshold (float convergence_threshold)
+LUM<PointT>::setConvergenceThreshold (float convergence_threshold)
{
convergence_threshold_ = convergence_threshold;
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline float
-
pcl::registration::LUM<PointT>::getConvergenceThreshold () const
+LUM<PointT>::getConvergenceThreshold () const
{
return (convergence_threshold_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT> typename
pcl::registration::LUM<PointT>::Vertex
-
pcl::registration::LUM<PointT>::addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose)
+
+template<typename PointT> typename LUM<PointT>::Vertex
+LUM<PointT>::addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose)
{
Vertex v = add_vertex (*slam_graph_);
(*slam_graph_)[v].cloud_ = cloud;
return (v);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline void
-
pcl::registration::LUM<PointT>::setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud)
+LUM<PointT>::setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud)
{
if (vertex >= getNumVertices ())
{
(*slam_graph_)[vertex].cloud_ = cloud;
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT> inline typename
pcl::registration::LUM<PointT>::PointCloudPtr
-
pcl::registration::LUM<PointT>::getPointCloud (const Vertex &vertex) const
+
+template<typename PointT> inline typename LUM<PointT>::PointCloudPtr
+LUM<PointT>::getPointCloud (const Vertex &vertex) const
{
if (vertex >= getNumVertices ())
{
return ((*slam_graph_)[vertex].cloud_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline void
-
pcl::registration::LUM<PointT>::setPose (const Vertex &vertex, const Eigen::Vector6f &pose)
+LUM<PointT>::setPose (const Vertex &vertex, const Eigen::Vector6f &pose)
{
if (vertex >= getNumVertices ())
{
(*slam_graph_)[vertex].pose_ = pose;
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline Eigen::Vector6f
-
pcl::registration::LUM<PointT>::getPose (const Vertex &vertex) const
+LUM<PointT>::getPose (const Vertex &vertex) const
{
if (vertex >= getNumVertices ())
{
return ((*slam_graph_)[vertex].pose_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline Eigen::Affine3f
-
pcl::registration::LUM<PointT>::getTransformation (const Vertex &vertex) const
+LUM<PointT>::getTransformation (const Vertex &vertex) const
{
Eigen::Vector6f pose = getPose (vertex);
return (pcl::getTransformation (pose (0), pose (1), pose (2), pose (3), pose (4), pose (5)));
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> void
-
pcl::registration::LUM<PointT>::setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs)
+LUM<PointT>::setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs)
{
if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex)
{
(*slam_graph_)[e].corrs_ = corrs;
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline pcl::CorrespondencesPtr
-
pcl::registration::LUM<PointT>::getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const
+LUM<PointT>::getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const
{
if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ())
{
return ((*slam_graph_)[e].corrs_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> void
-
pcl::registration::LUM<PointT>::compute ()
+LUM<PointT>::compute ()
{
int n = static_cast<int> (getNumVertices ());
if (n < 2)
}
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT> typename
pcl::registration::LUM<PointT>::PointCloudPtr
-
pcl::registration::LUM<PointT>::getTransformedCloud (const Vertex &vertex) const
+
+template<typename PointT> typename LUM<PointT>::PointCloudPtr
+LUM<PointT>::getTransformedCloud (const Vertex &vertex) const
{
PointCloudPtr out (new PointCloud);
pcl::transformPointCloud (*getPointCloud (vertex), *out, getTransformation (vertex));
return (out);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT> typename
pcl::registration::LUM<PointT>::PointCloudPtr
-
pcl::registration::LUM<PointT>::getConcatenatedCloud () const
+
+template<typename PointT> typename LUM<PointT>::PointCloudPtr
+LUM<PointT>::getConcatenatedCloud () const
{
PointCloudPtr out (new PointCloud);
typename SLAMGraph::vertex_iterator v, v_end;
return (out);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> void
-
pcl::registration::LUM<PointT>::computeEdge (const Edge &e)
+LUM<PointT>::computeEdge (const Edge &e)
{
// Get necessary local data from graph
PointCloudPtr source_cloud = (*slam_graph_)[source (e, *slam_graph_)].cloud_;
(*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointT> inline Eigen::Matrix6f
-
pcl::registration::LUM<PointT>::incidenceCorrection (const Eigen::Vector6f &pose)
+LUM<PointT>::incidenceCorrection (const Eigen::Vector6f &pose)
{
Eigen::Matrix6f out = Eigen::Matrix6f::Identity ();
float cx = std::cos (pose (3)), sx = sinf (pose (3)), cy = std::cos (pose (4)), sy = sinf (pose (4));
return (out);
}
+} // namespace registration
+} // namespace pcl
+
#define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>;
#endif // PCL_REGISTRATION_IMPL_LUM_HPP_
#ifndef PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_
#define PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointT, typename Scalar>
-
pcl::registration::MetaRegistration<PointT, Scalar>::MetaRegistration () :
+MetaRegistration<PointT, Scalar>::MetaRegistration () :
abs_transform_ (Matrix4::Identity ())
{}
template <typename PointT, typename Scalar> bool
-
pcl::registration::MetaRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& new_cloud, const Matrix4& delta_estimate)
+MetaRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& new_cloud, const Matrix4& delta_estimate)
{
assert (registration_);
return (converged);
}
-template <typename PointT, typename Scalar> inline typename
pcl::registration::MetaRegistration<PointT, Scalar>::Matrix4
-
pcl::registration::MetaRegistration<PointT, Scalar>::getAbsoluteTransform () const
+template <typename PointT, typename Scalar> inline typename MetaRegistration<PointT, Scalar>::Matrix4
+MetaRegistration<PointT, Scalar>::getAbsoluteTransform () const
{
return (abs_transform_);
}
template <typename PointT, typename Scalar> inline void
-
pcl::registration::MetaRegistration<PointT, Scalar>::reset ()
+MetaRegistration<PointT, Scalar>::reset ()
{
full_cloud_.reset ();
abs_transform_ = Matrix4::Identity ();
}
template <typename PointT, typename Scalar> inline void
-
pcl::registration::MetaRegistration<PointT, Scalar>::setRegistration (RegistrationPtr reg)
+MetaRegistration<PointT, Scalar>::setRegistration (RegistrationPtr reg)
{
registration_ = reg;
}
-template <typename PointT, typename Scalar> inline typename
pcl::registration::MetaRegistration<PointT, Scalar>::PointCloudConstPtr
-
pcl::registration::MetaRegistration<PointT, Scalar>::getMetaCloud () const
+template <typename PointT, typename Scalar> inline typename MetaRegistration<PointT, Scalar>::PointCloudConstPtr
+MetaRegistration<PointT, Scalar>::getMetaCloud () const
{
return full_cloud_;
}
+
+} // namespace registration
+} // namespace pcl
+
#endif /*PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_*/
+
#ifndef PCL_REGISTRATION_NDT_IMPL_H_
#define PCL_REGISTRATION_NDT_IMPL_H_
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template<typename PointSource, typename PointTarget>
-pcl::NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform ()
+NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform ()
: target_cells_ ()
, resolution_ (1.0f)
, step_size_ (0.1)
max_iterations_ = 35;
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> void
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
+NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
nr_iterations_ = 0;
converged_ = false;
trans_probability_ = score / static_cast<double> (input_->points.size ());
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> double
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
- Eigen::Matrix<double, 6, 6> &hessian,
- PointCloudSource &trans_cloud,
- Eigen::Matrix<double, 6, 1> &p,
- bool compute_hessian)
+NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
+ Eigen::Matrix<double, 6, 6> &hessian,
+ PointCloudSource &trans_cloud,
+ Eigen::Matrix<double, 6, 1> &p,
+ bool compute_hessian)
{
// Original Point and Transformed Point
PointSource x_pt, x_trans_pt;
return (score);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> void
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives (Eigen::Matrix<double, 6, 1> &p, bool compute_hessian)
+NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives (Eigen::Matrix<double, 6, 1> &p, bool compute_hessian)
{
// Simplified math for near 0 angles
double cx, cy, cz, sx, sy, sz;
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> void
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian)
+NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian)
{
// Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p.
// Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009]
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> double
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
- Eigen::Matrix<double, 6, 6> &hessian,
- Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv,
- bool compute_hessian)
+NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
+ Eigen::Matrix<double, 6, 6> &hessian,
+ Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv,
+ bool compute_hessian)
{
Eigen::Vector3d cov_dxd_pi;
// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
return (score_inc);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> void
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
- PointCloudSource &trans_cloud, Eigen::Matrix<double, 6, 1> &)
+NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
+ PointCloudSource &trans_cloud, Eigen::Matrix<double, 6, 1> &)
{
// Original Point and Transformed Point
PointSource x_pt, x_trans_pt;
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> void
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian, Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv)
+NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian, Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv)
{
Eigen::Vector3d cov_dxd_pi;
// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> bool
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT (double &a_l, double &f_l, double &g_l,
- double &a_u, double &f_u, double &g_u,
- double a_t, double f_t, double g_t)
+NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT (double &a_l, double &f_l, double &g_l,
+ double &a_u, double &f_u, double &g_u,
+ double a_t, double f_t, double g_t)
{
// Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994]
if (f_t > f_l)
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> double
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::trialValueSelectionMT (double a_l, double f_l, double g_l,
- double a_u, double f_u, double g_u,
- double a_t, double f_t, double g_t)
+NormalDistributionsTransform<PointSource, PointTarget>::trialValueSelectionMT (double a_l, double f_l, double g_l,
+ double a_u, double f_u, double g_u,
+ double a_t, double f_t, double g_t)
{
// Case 1 in Trial Value Selection [More, Thuente 1994]
if (f_t > f_l)
return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w));
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> double
-
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
- double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
- PointCloudSource &trans_cloud)
+NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
+ double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
+ PointCloudSource &trans_cloud)
{
// Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
double phi_0 = -score;
return (a_t);
}
+} // namespace pcl
+
#endif // PCL_REGISTRATION_NDT_IMPL_H_
+
/*
- * Software License Agreement (BSD License)
- *
- * Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2011-2012, Willow Garage, Inc.
- * Copyright (c) 2012-, Open Perception, Inc.
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the copyright holder(s) nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * $Id$
- *
- */
+* Software License Agreement (BSD License)
+*
+* Point Cloud Library (PCL) - www.pointclouds.org
+* Copyright (c) 2011-2012, Willow Garage, Inc.
+* Copyright (c) 2012-, Open Perception, Inc.
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the copyright holder(s) nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* $Id$
+*
+*/
+
#ifndef PCL_NDT_2D_IMPL_H_
#define PCL_NDT_2D_IMPL_H_
namespace pcl
{
- namespace ndt2d
+
+namespace ndt2d
+{
+/** \brief Class to store vector value and first and second derivatives
+ * (grad vector and hessian matrix), so they can be returned easily from
+ * functions
+ */
+template<unsigned N=3, typename T=double>
+struct ValueAndDerivatives
+{
+ ValueAndDerivatives () : hessian (), grad (), value () {}
+
+ Eigen::Matrix<T, N, N> hessian;
+ Eigen::Matrix<T, N, 1> grad;
+ T value;
+
+ static ValueAndDerivatives<N,T>
+ Zero ()
+ {
+ ValueAndDerivatives<N,T> r;
+ r.hessian = Eigen::Matrix<T, N, N>::Zero ();
+ r.grad = Eigen::Matrix<T, N, 1>::Zero ();
+ r.value = 0;
+ return r;
+ }
+
+ ValueAndDerivatives<N,T>&
+ operator+= (ValueAndDerivatives<N,T> const& r)
+ {
+ hessian += r.hessian;
+ grad += r.grad;
+ value += r.value;
+ return *this;
+ }
+};
+
+/** \brief A normal distribution estimation class.
+ *
+ * First the indices of of the points from a point cloud that should be
+ * modelled by the distribution are added with addIdx (...).
+ *
+ * Then estimateParams (...) uses the stored point indices to estimate the
+ * parameters of a normal distribution, and discards the stored indices.
+ *
+ * Finally the distriubution, and its derivatives, may be evaluated at any
+ * point using test (...).
+ */
+template <typename PointT>
+class NormalDist
+{
+ using PointCloud = pcl::PointCloud<PointT>;
+
+public:
+ NormalDist ()
+ : min_n_ (3), n_ (0)
+ {
+ }
+
+ /** \brief Store a point index to use later for estimating distribution parameters.
+ * \param[in] i Point index to store
+ */
+ void
+ addIdx (std::size_t i)
{
- /** \brief Class to store vector value and first and second derivatives
- * (grad vector and hessian matrix), so they can be returned easily from
- * functions
- */
- template<unsigned N=3, typename T=double>
- struct ValueAndDerivatives
+ pt_indices_.push_back (i);
+ }
+
+ /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared.
+ * \param[in] cloud Point cloud corresponding to indices passed to addIdx.
+ * \param[in] min_covar_eigvalue_mult Set the smallest eigenvalue to this times the largest.
+ */
+ void
+ estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001)
+ {
+ Eigen::Vector2d sx = Eigen::Vector2d::Zero ();
+ Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();
+
+ for (auto i = pt_indices_.cbegin (); i != pt_indices_.cend (); i++)
{
- ValueAndDerivatives () : hessian (), grad (), value () {}
-
- Eigen::Matrix<T, N, N> hessian;
- Eigen::Matrix<T, N, 1> grad;
- T value;
-
- static ValueAndDerivatives<N,T>
- Zero ()
- {
- ValueAndDerivatives<N,T> r;
- r.hessian = Eigen::Matrix<T, N, N>::Zero ();
- r.grad = Eigen::Matrix<T, N, 1>::Zero ();
- r.value = 0;
- return r;
- }
+ Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);
+ sx += p;
+ sxx += p * p.transpose ();
+ }
+
+ n_ = pt_indices_.size ();
+
+ if (n_ >= min_n_)
+ {
+ mean_ = sx / static_cast<double> (n_);
+ // Using maximum likelihood estimation as in the original paper
+ Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast<double> (n_) + mean_ * mean_.transpose ();
- ValueAndDerivatives<N,T>&
- operator+= (ValueAndDerivatives<N,T> const& r)
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver (covar);
+ if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1])
{
- hessian += r.hessian;
- grad += r.grad;
- value += r.value;
- return *this;
+ PCL_DEBUG ("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]);
+ Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();
+ Eigen::Matrix2d q = solver.eigenvectors ();
+ // set minimum smallest eigenvalue:
+ l (0,0) = l (1,1) * min_covar_eigvalue_mult;
+ covar = q * l * q.transpose ();
}
- };
-
- /** \brief A normal distribution estimation class.
- *
- * First the indices of of the points from a point cloud that should be
- * modelled by the distribution are added with addIdx (...).
- *
- * Then estimateParams (...) uses the stored point indices to estimate the
- * parameters of a normal distribution, and discards the stored indices.
- *
- * Finally the distriubution, and its derivatives, may be evaluated at any
- * point using test (...).
- */
- template <typename PointT>
- class NormalDist
- {
- using PointCloud = pcl::PointCloud<PointT>;
-
- public:
- NormalDist ()
- : min_n_ (3), n_ (0)
- {
- }
-
- /** \brief Store a point index to use later for estimating distribution parameters.
- * \param[in] i Point index to store
- */
- void
- addIdx (std::size_t i)
- {
- pt_indices_.push_back (i);
- }
-
- /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared.
- * \param[in] cloud Point cloud corresponding to indices passed to addIdx.
- * \param[in] min_covar_eigvalue_mult Set the smallest eigenvalue to this times the largest.
- */
- void
- estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001)
- {
- Eigen::Vector2d sx = Eigen::Vector2d::Zero ();
- Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();
-
- for (auto i = pt_indices_.cbegin (); i != pt_indices_.cend (); i++)
- {
- Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);
- sx += p;
- sxx += p * p.transpose ();
- }
-
- n_ = pt_indices_.size ();
-
- if (n_ >= min_n_)
- {
- mean_ = sx / static_cast<double> (n_);
- // Using maximum likelihood estimation as in the original paper
- Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast<double> (n_) + mean_ * mean_.transpose ();
-
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver (covar);
- if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1])
- {
- PCL_DEBUG ("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]);
- Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();
- Eigen::Matrix2d q = solver.eigenvectors ();
- // set minimum smallest eigenvalue:
- l (0,0) = l (1,1) * min_covar_eigvalue_mult;
- covar = q * l * q.transpose ();
- }
- covar_inv_ = covar.inverse ();
- }
-
- pt_indices_.clear ();
- }
-
- /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
- * \param[in] transformed_pt Location to evaluate at.
- * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
- * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
- * estimateParams must have been called after at least three points were provided, or this will return zero.
- *
- */
- ValueAndDerivatives<3,double>
- test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
- {
- if (n_ < min_n_)
- return ValueAndDerivatives<3,double>::Zero ();
-
- ValueAndDerivatives<3,double> r;
- const double x = transformed_pt.x;
- const double y = transformed_pt.y;
- const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y);
- const Eigen::Vector2d q = p_xy - mean_;
- const Eigen::RowVector2d qt_cvi (q.transpose () * covar_inv_);
- const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q));
- r.value = -exp_qt_cvi_q;
-
- Eigen::Matrix<double, 2, 3> jacobian;
- jacobian <<
- 1, 0, -(x * sin_theta + y*cos_theta),
- 0, 1, x * cos_theta - y*sin_theta;
-
- for (std::size_t i = 0; i < 3; i++)
- r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q;
-
- // second derivative only for i == j == 2:
- const Eigen::Vector2d d2q_didj (
- y * sin_theta - x*cos_theta,
- -(x * sin_theta + y*cos_theta)
- );
-
- for (std::size_t i = 0; i < 3; i++)
- for (std::size_t j = 0; j < 3; j++)
- r.hessian (i,j) = -exp_qt_cvi_q * (
- double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) +
- (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) +
- (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i))
- );
-
- return r;
- }
-
- protected:
- const std::size_t min_n_;
-
- std::size_t n_;
- std::vector<std::size_t> pt_indices_;
- Eigen::Vector2d mean_;
- Eigen::Matrix2d covar_inv_;
- };
-
- /** \brief Build a set of normal distributions modelling a 2D point cloud,
- * and provide the value and derivatives of the model at any point via the
- * test (...) function.
- */
- template <typename PointT>
- class NDTSingleGrid: public boost::noncopyable
- {
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudConstPtr = typename PointCloud::ConstPtr;
- using NormalDist = pcl::ndt2d::NormalDist<PointT>;
-
- public:
- NDTSingleGrid (PointCloudConstPtr cloud,
- const Eigen::Vector2f& about,
- const Eigen::Vector2f& extent,
- const Eigen::Vector2f& step)
- : min_ (about - extent), max_ (min_ + 2*extent), step_ (step),
- cells_ ((max_[0]-min_[0]) / step_[0],
- (max_[1]-min_[1]) / step_[1]),
- normal_distributions_ (cells_[0], cells_[1])
- {
- // sort through all points, assigning them to distributions:
- std::size_t used_points = 0;
- for (std::size_t i = 0; i < cloud->size (); i++)
- if (NormalDist* n = normalDistForPoint (cloud->at (i)))
- {
- n->addIdx (i);
- used_points++;
- }
-
- PCL_DEBUG ("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n", cells_[0], cells_[1], used_points, cloud->size ());
-
- // then bake the distributions such that they approximate the
- // points (and throw away memory of the points)
- for (int x = 0; x < cells_[0]; x++)
- for (int y = 0; y < cells_[1]; y++)
- normal_distributions_.coeffRef (x,y).estimateParams (*cloud);
- }
-
- /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
- * \param[in] transformed_pt Location to evaluate at.
- * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
- * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
- */
- ValueAndDerivatives<3,double>
- test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
- {
- const NormalDist* n = normalDistForPoint (transformed_pt);
- // index is in grid, return score from the normal distribution from
- // the correct part of the grid:
- if (n)
- return n->test (transformed_pt, cos_theta, sin_theta);
- return ValueAndDerivatives<3,double>::Zero ();
- }
-
- protected:
- /** \brief Return the normal distribution covering the location of point p
- * \param[in] p a point
- */
- NormalDist*
- normalDistForPoint (PointT const& p) const
- {
- // this would be neater in 3d...
- Eigen::Vector2f idxf;
- for (std::size_t i = 0; i < 2; i++)
- idxf[i] = (p.getVector3fMap ()[i] - min_[i]) / step_[i];
- Eigen::Vector2i idxi = idxf.cast<int> ();
- for (std::size_t i = 0; i < 2; i++)
- if (idxi[i] >= cells_[i] || idxi[i] < 0)
- return nullptr;
- // const cast to avoid duplicating this function in const and
- // non-const variants...
- return const_cast<NormalDist*> (&normal_distributions_.coeffRef (idxi[0], idxi[1]));
- }
-
- Eigen::Vector2f min_;
- Eigen::Vector2f max_;
- Eigen::Vector2f step_;
- Eigen::Vector2i cells_;
-
- Eigen::Matrix<NormalDist, Eigen::Dynamic, Eigen::Dynamic> normal_distributions_;
- };
-
- /** \brief Build a Normal Distributions Transform of a 2D point cloud. This
- * consists of the sum of four overlapping models of the original points
- * with normal distributions.
- * The value and derivatives of the model at any point can be evaluated
- * with the test (...) function.
- */
- template <typename PointT>
- class NDT2D: public boost::noncopyable
+ covar_inv_ = covar.inverse ();
+ }
+
+ pt_indices_.clear ();
+ }
+
+ /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
+ * \param[in] transformed_pt Location to evaluate at.
+ * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
+ * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
+ * estimateParams must have been called after at least three points were provided, or this will return zero.
+ *
+ */
+ ValueAndDerivatives<3,double>
+ test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
+ {
+ if (n_ < min_n_)
+ return ValueAndDerivatives<3,double>::Zero ();
+
+ ValueAndDerivatives<3,double> r;
+ const double x = transformed_pt.x;
+ const double y = transformed_pt.y;
+ const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y);
+ const Eigen::Vector2d q = p_xy - mean_;
+ const Eigen::RowVector2d qt_cvi (q.transpose () * covar_inv_);
+ const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q));
+ r.value = -exp_qt_cvi_q;
+
+ Eigen::Matrix<double, 2, 3> jacobian;
+ jacobian <<
+ 1, 0, -(x * sin_theta + y*cos_theta),
+ 0, 1, x * cos_theta - y*sin_theta;
+
+ for (std::size_t i = 0; i < 3; i++)
+ r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q;
+
+ // second derivative only for i == j == 2:
+ const Eigen::Vector2d d2q_didj (
+ y * sin_theta - x*cos_theta,
+ -(x * sin_theta + y*cos_theta)
+ );
+
+ for (std::size_t i = 0; i < 3; i++)
+ for (std::size_t j = 0; j < 3; j++)
+ r.hessian (i,j) = -exp_qt_cvi_q * (
+ double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) +
+ (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) +
+ (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i))
+ );
+
+ return r;
+ }
+
+protected:
+ const std::size_t min_n_;
+
+ std::size_t n_;
+ std::vector<std::size_t> pt_indices_;
+ Eigen::Vector2d mean_;
+ Eigen::Matrix2d covar_inv_;
+};
+
+/** \brief Build a set of normal distributions modelling a 2D point cloud,
+ * and provide the value and derivatives of the model at any point via the
+ * test (...) function.
+ */
+template <typename PointT>
+class NDTSingleGrid: public boost::noncopyable
+{
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudConstPtr = typename PointCloud::ConstPtr;
+ using NormalDist = pcl::ndt2d::NormalDist<PointT>;
+
+public:
+ NDTSingleGrid (PointCloudConstPtr cloud,
+ const Eigen::Vector2f& about,
+ const Eigen::Vector2f& extent,
+ const Eigen::Vector2f& step)
+ : min_ (about - extent), max_ (min_ + 2*extent), step_ (step),
+ cells_ ((max_[0]-min_[0]) / step_[0],
+ (max_[1]-min_[1]) / step_[1]),
+ normal_distributions_ (cells_[0], cells_[1])
+ {
+ // sort through all points, assigning them to distributions:
+ std::size_t used_points = 0;
+ for (std::size_t i = 0; i < cloud->size (); i++)
+ if (NormalDist* n = normalDistForPoint (cloud->at (i)))
{
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudConstPtr = typename PointCloud::ConstPtr;
- using SingleGrid = NDTSingleGrid<PointT>;
-
- public:
- /** \brief
- * \param[in] cloud the input point cloud
- * \param[in] about Centre of the grid for normal distributions model
- * \param[in] extent Extent of grid for normal distributions model
- * \param[in] step Size of region that each normal distribution will model
- */
- NDT2D (PointCloudConstPtr cloud,
- const Eigen::Vector2f& about,
- const Eigen::Vector2f& extent,
- const Eigen::Vector2f& step)
- {
- Eigen::Vector2f dx (step[0]/2, 0);
- Eigen::Vector2f dy (0, step[1]/2);
- single_grids_[0].reset(new SingleGrid (cloud, about, extent, step));
- single_grids_[1].reset(new SingleGrid (cloud, about +dx, extent, step));
- single_grids_[2].reset(new SingleGrid (cloud, about +dy, extent, step));
- single_grids_[3].reset(new SingleGrid (cloud, about +dx+dy, extent, step));
- }
-
- /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
- * \param[in] transformed_pt Location to evaluate at.
- * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
- * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
- */
- ValueAndDerivatives<3,double>
- test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
- {
- ValueAndDerivatives<3,double> r = ValueAndDerivatives<3,double>::Zero ();
- for (const auto &single_grid : single_grids_)
- r += single_grid->test (transformed_pt, cos_theta, sin_theta);
- return r;
- }
-
- protected:
- std::shared_ptr<SingleGrid> single_grids_[4];
- };
-
- } // namespace ndt2d
+ n->addIdx (i);
+ used_points++;
+ }
+
+ PCL_DEBUG ("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n", cells_[0], cells_[1], used_points, cloud->size ());
+
+ // then bake the distributions such that they approximate the
+ // points (and throw away memory of the points)
+ for (int x = 0; x < cells_[0]; x++)
+ for (int y = 0; y < cells_[1]; y++)
+ normal_distributions_.coeffRef (x,y).estimateParams (*cloud);
+ }
+
+ /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
+ * \param[in] transformed_pt Location to evaluate at.
+ * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
+ * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
+ */
+ ValueAndDerivatives<3,double>
+ test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
+ {
+ const NormalDist* n = normalDistForPoint (transformed_pt);
+ // index is in grid, return score from the normal distribution from
+ // the correct part of the grid:
+ if (n)
+ return n->test (transformed_pt, cos_theta, sin_theta);
+ return ValueAndDerivatives<3,double>::Zero ();
+ }
+
+protected:
+ /** \brief Return the normal distribution covering the location of point p
+ * \param[in] p a point
+ */
+ NormalDist*
+ normalDistForPoint (PointT const& p) const
+ {
+ // this would be neater in 3d...
+ Eigen::Vector2f idxf;
+ for (std::size_t i = 0; i < 2; i++)
+ idxf[i] = (p.getVector3fMap ()[i] - min_[i]) / step_[i];
+ Eigen::Vector2i idxi = idxf.cast<int> ();
+ for (std::size_t i = 0; i < 2; i++)
+ if (idxi[i] >= cells_[i] || idxi[i] < 0)
+ return nullptr;
+ // const cast to avoid duplicating this function in const and
+ // non-const variants...
+ return const_cast<NormalDist*> (&normal_distributions_.coeffRef (idxi[0], idxi[1]));
+ }
+
+ Eigen::Vector2f min_;
+ Eigen::Vector2f max_;
+ Eigen::Vector2f step_;
+ Eigen::Vector2i cells_;
+
+ Eigen::Matrix<NormalDist, Eigen::Dynamic, Eigen::Dynamic> normal_distributions_;
+};
+
+/** \brief Build a Normal Distributions Transform of a 2D point cloud. This
+ * consists of the sum of four overlapping models of the original points
+ * with normal distributions.
+ * The value and derivatives of the model at any point can be evaluated
+ * with the test (...) function.
+ */
+template <typename PointT>
+class NDT2D: public boost::noncopyable
+{
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudConstPtr = typename PointCloud::ConstPtr;
+ using SingleGrid = NDTSingleGrid<PointT>;
+
+public:
+ /** \brief
+ * \param[in] cloud the input point cloud
+ * \param[in] about Centre of the grid for normal distributions model
+ * \param[in] extent Extent of grid for normal distributions model
+ * \param[in] step Size of region that each normal distribution will model
+ */
+ NDT2D (PointCloudConstPtr cloud,
+ const Eigen::Vector2f& about,
+ const Eigen::Vector2f& extent,
+ const Eigen::Vector2f& step)
+ {
+ Eigen::Vector2f dx (step[0]/2, 0);
+ Eigen::Vector2f dy (0, step[1]/2);
+ single_grids_[0].reset(new SingleGrid (cloud, about, extent, step));
+ single_grids_[1].reset(new SingleGrid (cloud, about +dx, extent, step));
+ single_grids_[2].reset(new SingleGrid (cloud, about +dy, extent, step));
+ single_grids_[3].reset(new SingleGrid (cloud, about +dx+dy, extent, step));
+ }
+
+ /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
+ * \param[in] transformed_pt Location to evaluate at.
+ * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
+ * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
+ */
+ ValueAndDerivatives<3,double>
+ test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
+ {
+ ValueAndDerivatives<3,double> r = ValueAndDerivatives<3,double>::Zero ();
+ for (const auto &single_grid : single_grids_)
+ r += single_grid->test (transformed_pt, cos_theta, sin_theta);
+ return r;
+ }
+
+protected:
+ std::shared_ptr<SingleGrid> single_grids_[4];
+};
+
+} // namespace ndt2d
} // namespace pcl
namespace Eigen
{
- /* This NumTraits specialisation is necessary because NormalDist is used as
- * the element type of an Eigen Matrix.
- */
- template<typename PointT> struct NumTraits<pcl::ndt2d::NormalDist<PointT> >
+
+/* This NumTraits specialisation is necessary because NormalDist is used as
+* the element type of an Eigen Matrix.
+*/
+template<typename PointT>
+struct NumTraits<pcl::ndt2d::NormalDist<PointT> >
+{
+ using Real = double;
+ using Literal = double;
+ static Real dummy_precision () { return 1.0; }
+ enum
{
- using Real = double;
- using Literal = double;
- static Real dummy_precision () { return 1.0; }
- enum {
- IsComplex = 0,
- IsInteger = 0,
- IsSigned = 0,
- RequireInitialization = 1,
- ReadCost = 1,
- AddCost = 1,
- MulCost = 1
- };
+ IsComplex = 0,
+ IsInteger = 0,
+ IsSigned = 0,
+ RequireInitialization = 1,
+ ReadCost = 1,
+ AddCost = 1,
+ MulCost = 1
};
-}
+};
+
+} // namespace Eigen
+
+
+namespace pcl
+{
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget> void
-
pcl::NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
+NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
PointCloudSource intm_cloud = output;
{
transformation_ = guess;
transformPointCloud (output, intm_cloud, transformation_);
- }
+ }
// build Normal Distribution Transform of target cloud:
ndt2d::NDT2D<PointTarget> target_ndt (target_, grid_centre_, grid_extent_, grid_step_);
-
+
// can't seem to use .block<> () member function on transformation_
- // directly... gcc bug?
+ // directly... gcc bug?
Eigen::Matrix4f& transformation = transformation_;
{
const double cos_theta = std::cos (xytheta_transformation[2]);
const double sin_theta = std::sin (xytheta_transformation[2]);
- previous_transformation_ = transformation;
+ previous_transformation_ = transformation;
ndt2d::ValueAndDerivatives<3, double> score = ndt2d::ValueAndDerivatives<3, double>::Zero ();
for (std::size_t i = 0; i < intm_cloud.size (); i++)
score += target_ndt.test (intm_cloud[i], cos_theta, sin_theta);
-
+
PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score %f (x=%f,y=%f,r=%f)\n",
float (score.value), xytheta_transformation[0], xytheta_transformation[1], xytheta_transformation[2]
);
assert (solver.eigenvalues ()[0].real () >= 0 &&
solver.eigenvalues ()[1].real () >= 0 &&
solver.eigenvalues ()[2].real () >= 0);
-
+
Eigen::Vector3d delta_transformation (-score.hessian.inverse () * score.grad);
Eigen::Vector3d new_transformation = xytheta_transformation + newton_lambda_.cwiseProduct (delta_transformation);
xytheta_transformation = new_transformation;
-
+
// update transformation matrix from x, y, theta:
transformation.block<3,3> (0,0).matrix () = Eigen::Matrix3f (Eigen::AngleAxisf (static_cast<float> (xytheta_transformation[2]), Eigen::Vector3f::UnitZ ()));
transformation.block<3,1> (0,3).matrix () = Eigen::Vector3f (static_cast<float> (xytheta_transformation[0]), static_cast<float> (xytheta_transformation[1]), 0.0f);
PCL_ERROR ("[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n");
break;
}
-
+
transformPointCloud (output, intm_cloud, transformation);
nr_iterations_++;
-
+
if (update_visualizer_)
update_visualizer_ (output, *indices_, *target_, *indices_);
output = intm_cloud;
}
+} // namespace pcl
+
#endif // PCL_NDT_2D_IMPL_H_
-
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
#define PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+namespace pcl
+{
+
template <typename GraphT, typename PointT> void
-pcl::PairwiseGraphRegistration<GraphT, PointT>::computeRegistration ()
+PairwiseGraphRegistration<GraphT, PointT>::computeRegistration ()
{
if (!registration_method_)
{
registration_method_->setInputTarget (boost::get_cloud<PointT> (last_aligned_vertex_, *(graph_handler_->getGraph ())));
}
}
+
+} // namespace pcl
+
#endif //PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
+
#include <pcl/pcl_macros.h>
#include <pcl/console/print.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointFeature> float
-
pcl::PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a,
- const PyramidFeatureHistogramPtr &pyramid_b)
+PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a,
+ const PyramidFeatureHistogramPtr &pyramid_b)
{
// do a few consistency checks before and during the computation
if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature>
-
pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram () :
+PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram () :
nr_dimensions (0), nr_levels (0), nr_features (0),
feature_representation_ (new DefaultPointRepresentation<PointFeature>),
is_computed_ (false),
{
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointFeature> void
-
pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel ()
+PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel ()
{
std::size_t total_vector_size = 1;
for (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature> bool
-
pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
+PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
{
// a few consistency checks before starting the computations
if (!PCLBase<PointFeature>::initCompute ())
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature> unsigned int&
-
pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<std::size_t> &access,
- std::size_t &level)
+PyramidFeatureHistogram<PointFeature>::at (std::vector<std::size_t> &access,
+ std::size_t &level)
{
if (access.size () != nr_dimensions)
{
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature> unsigned int&
-
pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
- std::size_t &level)
+PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
+ std::size_t &level)
{
if (feature.size () != nr_dimensions)
{
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature> void
-
pcl::PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature,
- std::vector<float> &feature_vector)
+PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature,
+ std::vector<float> &feature_vector)
{
// convert feature to vector representation
feature_vector.resize (feature_representation_->getNumberOfDimensions ());
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature> void
-
pcl::PyramidFeatureHistogram<PointFeature>::compute ()
+PyramidFeatureHistogram<PointFeature>::compute ()
{
if (!initializeHistogram ())
return;
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointFeature> void
-
pcl::PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
+PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
{
for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
at (feature, level_i) ++;
}
+} // namespace pcl
+
#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
#endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
+
*
*/
-///////////////////////////////////////////////////////////////////////////////////////////
+#pragma once
+
+namespace pcl
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget (const PointCloudTargetConstPtr &cloud)
+Registration<PointSource, PointTarget, Scalar>::setInputTarget (const PointCloudTargetConstPtr &cloud)
{
if (cloud->points.empty ())
{
target_cloud_updated_ = true;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> bool
-
pcl::Registration<PointSource, PointTarget, Scalar>::initCompute ()
+Registration<PointSource, PointTarget, Scalar>::initCompute ()
{
if (!target_)
{
target_cloud_updated_ = false;
}
-
// Update the correspondence estimation
if (correspondence_estimation_)
{
correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_);
correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_);
}
-
+
// Note: we /cannot/ update the search method on all correspondence rejectors, because we know
// nothing about them. If they should be cached, they must be cached individually.
return (PCLBase<PointSource>::initCompute ());
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> bool
-
pcl::Registration<PointSource, PointTarget, Scalar>::initComputeReciprocal ()
+Registration<PointSource, PointTarget, Scalar>::initComputeReciprocal ()
{
if (!input_)
{
return (true);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline double
-
pcl::Registration<PointSource, PointTarget, Scalar>::getFitnessScore (
+Registration<PointSource, PointTarget, Scalar>::getFitnessScore (
const std::vector<float> &distances_a,
const std::vector<float> &distances_b)
{
return (static_cast<double> ((map_a - map_b).sum ()) / static_cast<double> (nr_elem));
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline double
-
pcl::Registration<PointSource, PointTarget, Scalar>::getFitnessScore (double max_range)
+Registration<PointSource, PointTarget, Scalar>::getFitnessScore (double max_range)
{
-
double fitness_score = 0.0;
// Transform the input dataset using the final transformation
{
// Find its nearest neighbor in the target
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
-
+
// Deal with occlusions (incomplete targets)
if (nn_dists[0] <= max_range)
{
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output)
+Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output)
{
align (output, Matrix4::Identity ());
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
+Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
{
- if (!initCompute ())
+ if (!initCompute ())
return;
// Resize the output dataset
deinitCompute ();
}
+} // namespace pcl
+
#ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
#define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features)
+
+namespace pcl
+{
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features)
{
if (features == nullptr || features->empty ())
{
input_features_ = features;
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features)
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features)
{
if (features == nullptr || features->empty ())
{
feature_tree_->setInputCloud (target_features_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples (
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples (
const PointCloudSource &cloud, int nr_samples, std::vector<int> &sample_indices)
{
if (nr_samples > static_cast<int> (cloud.points.size ()))
nr_samples, cloud.points.size ());
return;
}
-
+
sample_indices.resize (nr_samples);
int temp_sample;
{
// Select a random number
sample_indices[i] = getRandomIndex (static_cast<int> (cloud.points.size ()) - i);
-
+
// Run trough list of numbers, starting at the lowest, to avoid duplicates
for (int j = 0; j < i; j++)
{
temp_sample = sample_indices[i];
for (int k = i; k > j; k--)
sample_indices[k] = sample_indices[k - 1];
-
+
sample_indices[j] = temp_sample;
break;
}
}
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
const std::vector<int> &sample_indices,
std::vector<std::vector<int> >& similar_features,
std::vector<int> &corresponding_indices)
// Allocate results
corresponding_indices.resize (sample_indices.size ());
std::vector<float> nn_distances (k_correspondences_);
-
+
// Loop over the sampled features
for (std::size_t i = 0; i < sample_indices.size (); ++i)
{
// Current feature index
const int idx = sample_indices[i];
-
+
// Find the k nearest feature neighbors to the sampled input feature if they are not in the cache already
if (similar_features[idx].empty ())
feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances);
}
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
// Some sanity checks first
if (!input_features_)
inlier_fraction_);
return;
}
-
+
const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
{
similarity_threshold);
return;
}
-
+
if (k_correspondences_ <= 0)
{
PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
k_correspondences_);
return;
}
-
+
// Initialize prerejector (similarity threshold already set to default value in constructor)
correspondence_rejector_poly_->setInputSource (input_);
correspondence_rejector_poly_->setInputTarget (target_);
correspondence_rejector_poly_->setCardinality (nr_samples_);
int num_rejections = 0; // For debugging
-
+
// Initialize results
final_transformation_ = guess;
inliers_.clear ();
float lowest_error = std::numeric_limits<float>::max ();
converged_ = false;
-
+
// Temporaries
std::vector<int> inliers;
float inlier_fraction;
float error;
-
+
// If guess is not the Identity matrix we check it
if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
{
getFitness (inliers, error);
inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
-
+
if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
{
inliers_ = inliers;
converged_ = true;
}
}
-
+
// Feature correspondence cache
std::vector<std::vector<int> > similar_features (input_->size ());
-
+
// Start
for (int i = 0; i < max_iterations_; ++i)
{
// Temporary containers
std::vector<int> sample_indices;
std::vector<int> corresponding_indices;
-
+
// Draw nr_samples_ random samples
selectSamples (*input_, nr_samples_, sample_indices);
-
+
// Find corresponding features in the target cloud
findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
-
+
// Apply prerejection
if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
{
// Estimate the transform from the correspondences, write to transformation_
transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
-
+
// Take a backup of previous result
const Matrix4 final_transformation_prev = final_transformation_;
-
+
// Set final result to current transformation
final_transformation_ = transformation_;
-
+
// Transform the input and compute the error (uses input_ and final_transformation_)
getFitness (inliers, error);
-
+
// Restore previous result
final_transformation_ = final_transformation_prev;
// Apply the final transformation
if (converged_)
transformPointCloud (*input_, output, final_transformation_);
-
+
// Debug output
PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
getClassName ().c_str (), num_rejections, max_iterations_);
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-
pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness (std::vector<int>& inliers, float& fitness_score)
+
+template <typename PointSource, typename PointTarget, typename FeatureT> void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness (std::vector<int>& inliers, float& fitness_score)
{
// Initialize variables
inliers.clear ();
inliers.reserve (input_->size ());
fitness_score = 0.0f;
-
+
// Use squared distance for comparison with NN search results
const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
PointCloudSource input_transformed;
input_transformed.resize (input_->size ());
transformPointCloud (*input_, input_transformed, final_transformation_);
-
+
// For each point in the source dataset
for (std::size_t i = 0; i < input_transformed.points.size (); ++i)
{
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
-
+
// Check if point is an inlier
if (nn_dists[0] < max_range)
{
// Update inliers
inliers.push_back (static_cast<int> (i));
-
+
// Update fitness score
fitness_score += nn_dists[0];
}
fitness_score = std::numeric_limits<float>::max ();
}
+} // namespace pcl
+
#endif
* POSSIBILITY OF SUCH DAMAGE.
*
*/
+
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
}
-///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1> ¢roid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
// Assemble the correlation matrix H = source * target'
Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);
-
+
float angle = std::atan2 ((H (0, 1) - H (1, 0)), (H(0, 0) + H (1, 1)));
-
+
Eigen::Matrix<Scalar, 3, 3> R (Eigen::Matrix<Scalar, 3, 3>::Identity ());
R (0, 0) = R (1, 1) = std::cos (angle);
R (0, 1) = -std::sin (angle);
transformation_matrix.block (0, 3, 3, 1).matrix () = centroid_tgt.head (3) - Rc;
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
+
*
*
*/
+
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
#include <pcl/common/eigen.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
transformation_matrix(2,3) = -t.z();
}
+} // namespace registration
+} // namespace pcl
+
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ */
+
*
*
*/
+
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
#include <pcl/common/eigen.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
transformation_matrix (2, 3) = - t.z ();
}
+} // namespace registration
+} // namespace pcl
+
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ */
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
+
#include <pcl/cloud_iterator.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
ConstCloudIterator<PointSource> source_it (cloud_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
- estimateRigidTransformation (source_it, target_it, transformation_matrix);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
- estimateRigidTransformation (source_it, target_it, transformation_matrix);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
- estimateRigidTransformation (source_it, target_it, transformation_matrix);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
const double & tx, const double & ty, const double & tz,
Matrix4 &transformation_matrix) const
{
- // Construct the transformation matrix from rotation and translation
+ // Construct the transformation matrix from rotation and translation
transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
transformation_matrix (0, 0) = static_cast<Scalar> ( std::cos (gamma) * std::cos (beta));
transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
transformation_matrix (3, 3) = static_cast<Scalar> (1);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
{
using Vector6d = Eigen::Matrix<double, 6, 1>;
!std::isfinite (target_it->normal_z))
{
++target_it;
- ++source_it;
+ ++source_it;
continue;
}
const float & nz = target_it->normal[2];
double a = nz*sy - ny*sz;
- double b = nx*sz - nz*sx;
+ double b = nx*sz - nz*sx;
double c = ny*sx - nx*sy;
-
+
// 0 1 2 3 4 5
// 6 7 8 9 10 11
// 12 13 14 15 16 17
// 18 19 20 21 22 23
// 24 25 26 27 28 29
// 30 31 32 33 34 35
-
+
ATA.coeffRef (0) += a * a;
ATA.coeffRef (1) += a * b;
ATA.coeffRef (2) += a * c;
ATb.coeffRef (5) += nz * d;
++target_it;
- ++source_it;
+ ++source_it;
}
+
ATA.coeffRef (6) = ATA.coeff (1);
ATA.coeffRef (12) = ATA.coeff (2);
ATA.coeffRef (13) = ATA.coeff (8);
// Solve A*x = b
Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
-
+
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
+
+} // namespace registration
+} // namespace pcl
+
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
+
#include <pcl/cloud_iterator.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
const double & tx, const double & ty, const double & tz,
Matrix4 &transformation_matrix) const
{
- // Construct the transformation matrix from rotation and translation
+ // Construct the transformation matrix from rotation and translation
transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
transformation_matrix (0, 0) = static_cast<Scalar> ( std::cos (gamma) * std::cos (beta));
transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
transformation_matrix (3, 3) = static_cast<Scalar> (1);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
+TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
typename std::vector<Scalar>::const_iterator& weights_it,
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
+
+} // namespace registration
+} // namespace pcl
+
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ */
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
#include <pcl/common/eigen.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
cloud_tgt (2, i) = target_it->z;
++target_it;
}
-
+
// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
}
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1> ¢roid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc;
}
+} // namespace registration
+} // namespace pcl
+
//#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD<T,U>;
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ */
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
+TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1> ¢roid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();
// rotated cloud
- Eigen::Matrix<Scalar, 4, 4> R4;
+ Eigen::Matrix<Scalar, 4, 4> R4;
R4.block (0, 0, 3, 3) = R;
R4 (0, 3) = 0;
R4 (1, 3) = 0;
R4 (3, 3) = 1;
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R4 * cloud_src_demean;
-
- float scale1, scale2;
- double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f;
+
+ double sum_ss = 0.0f, sum_tt = 0.0f;
for (unsigned corrIdx = 0; corrIdx < cloud_src_demean.cols (); ++corrIdx)
{
sum_ss += cloud_src_demean (0, corrIdx) * cloud_src_demean (0, corrIdx);
sum_ss += cloud_src_demean (1, corrIdx) * cloud_src_demean (1, corrIdx);
sum_ss += cloud_src_demean (2, corrIdx) * cloud_src_demean (2, corrIdx);
-
- sum_tt += cloud_tgt_demean (0, corrIdx) * cloud_tgt_demean (0, corrIdx);
- sum_tt += cloud_tgt_demean (1, corrIdx) * cloud_tgt_demean (1, corrIdx);
- sum_tt += cloud_tgt_demean (2, corrIdx) * cloud_tgt_demean (2, corrIdx);
-
- sum_tt_ += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx);
- sum_tt_ += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx);
- sum_tt_ += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx);
+
+ sum_tt += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx);
+ sum_tt += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx);
+ sum_tt += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx);
}
-
- scale1 = sqrt (sum_tt / sum_ss);
- scale2 = sum_tt_ / sum_ss;
- float scale = scale2;
+
+ float scale = sum_tt / sum_ss;
transformation_matrix.topLeftCorner (3, 3) = scale * R;
const Eigen::Matrix<Scalar, 3, 1> Rc (scale * R * centroid_src.head (3));
transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc;
}
+} // namespace registration
+} // namespace pcl
+
//#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD<T,U>;
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ */
#include <pcl/cloud_iterator.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
ConstCloudIterator<PointSource> source_it (cloud_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
- estimateRigidTransformation (source_it, target_it, transformation_matrix);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> void
-
pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
- estimateRigidTransformation (source_it, target_it, transformation_matrix);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
- estimateRigidTransformation (source_it, target_it, transformation_matrix);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
constructTransformationMatrix (const Vector6 ¶meters,
Matrix4 &transformation_matrix) const
{
- // Construct the transformation matrix from rotation and translation
+ // Construct the transformation matrix from rotation and translation
const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
transformation_matrix = transform.matrix ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
{
using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
Vector6 v;
v << (p + q).cross (n), n;
M.rankUpdate (v);
-
+
ATb += v * (q - p).dot (n);
}
// Solve A*x = b
const Vector6 x = M.ldlt ().solve (ATb);
-
+
// Construct the transformation matrix from x
constructTransformationMatrix (x, transformation_matrix);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointSource, typename PointTarget, typename Scalar> inline void
-
pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
{
enforce_same_direction_normals_ = enforce_same_direction_normals;
}
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename Scalar> inline bool
-
pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
+
+template <typename PointSource, typename PointTarget, typename Scalar> inline bool
+TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
getEnforceSameDirectionNormals ()
{
return enforce_same_direction_normals_;
}
+
+} // namespace registration
+} // namespace pcl
+
* $Id$
*
*/
+
#ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
#define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
-/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace registration
+{
+
template <typename PointSource, typename PointTarget, typename Scalar> double
-
pcl::registration::TransformationValidationEuclidean<PointSource, PointTarget, Scalar>::validateTransformation (
+TransformationValidationEuclidean<PointSource, PointTarget, Scalar>::validateTransformation (
const PointCloudSourceConstPtr &cloud_src,
const PointCloudTargetConstPtr &cloud_tgt,
const Matrix4 &transformation_matrix) const
{
// Find its nearest neighbor in the target
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
-
+
// Deal with occlusions (incomplete targets)
if (nn_dists[0] > max_range_)
continue;
return (std::numeric_limits<double>::max ());
}
+} // namespace registration
+} // namespace pcl
+
#endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/pcl_base.h>
#include <pcl/registration/eigen.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/registration.h>
#include <pcl/common/common.h>
virtual ~MetaRegistration () {};
/** \brief Register new point cloud
- * \note You have to set a valid registration object with @ref setICP before using this
+ * \note You have to set a valid registration object with \ref setRegistration before using this
* \param[in] cloud point cloud to register
* \param[in] delta_estimate estimated transform between last registered cloud and this one
- * \return true if ICP converged
+ * \return true if registration converged
*/
bool
registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ());
inline Matrix4
getAbsoluteTransform () const;
- /** \brief Reset MetaICP without resetting registration_ */
+ /** \brief Reset MetaRegistration without resetting registration_ */
inline void
reset ();
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/registration.h>
#include <pcl/filters/voxel_grid_covariance.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/registration.h>
/** \brief Returns the angle discretization step parameter (the step value between each bin of the hash map for the angular values) */
inline float
- getAngleDiscretizationStep () { return angle_discretization_step_; }
+ getAngleDiscretizationStep () const { return angle_discretization_step_; }
/** \brief Returns the distance discretization step parameter (the step value between each bin of the hash map for the distance values) */
inline float
- getDistanceDiscretizationStep () { return distance_discretization_step_; }
+ getDistanceDiscretizationStep () const { return distance_discretization_step_; }
/** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */
inline float
- getModelDiameter () { return max_dist_; }
+ getModelDiameter () const { return max_dist_; }
std::vector <std::vector <float> > alpha_m_;
private:
// PCL includes
#include <pcl/pcl_base.h>
#include <pcl/common/transforms.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/search/kdtree.h>
#include <pcl/registration/boost.h>
private:
/** \brief The point representation used (internal). */
PointRepresentationConstPtr point_representation_;
+
+ /**
+ * \brief Remove from public API in favor of \ref setInputSource
+ *
+ * Still gives the correct result (with a warning)
+ */
+ void setInputCloud (const PointCloudSourceConstPtr &cloud) override
+ {
+ PCL_WARN ("[pcl::registration::Registration] setInputCloud is deprecated."
+ "Please use setInputSource instead.\n");
+ setInputSource (cloud);
+ }
+
public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/transformation_estimation.h>
#include <pcl/registration/warp_point_rigid.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>
#include <pcl/registration/warp_point_rigid.h>
/** \brief @b TransformationEstimationSymmetricPointToPlaneLLS implements a Linear Least Squares (LLS) approximation
* for minimizing the symmetric point-to-plane distance between two clouds of corresponding points with normals.
*
- * For additional details, see
+ * For additional details, see
* "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004
* "A Symmetric Objective Function for ICP", Szymon Rusinkiewicz, 2019
*
using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
-
+
TransformationEstimationSymmetricPointToPlaneLLS () : enforce_same_direction_normals_ (true) {};
~TransformationEstimationSymmetricPointToPlaneLLS () {};
*/
inline void
setEnforceSameDirectionNormals (bool enforce_same_direction_normals);
-
+
/** \brief Obtain whether source or target normals are negated on a per-point basis such that they point in the same direction or not */
inline bool
getEnforceSameDirectionNormals ();
protected:
-
+
/** \brief Estimate a rigid rotation transformation between a source and a target
* \param[in] source_it an iterator over the source point cloud dataset
* \param[in] target_it an iterator over the target point cloud dataset
* \param[out] transformation_matrix the resultant transformation matrix
*/
- void
- estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
- ConstCloudIterator<PointTarget>& target_it,
+ void
+ estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
+ ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const;
/** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation.
- * \param[in] (alpha, beta, gamma, tx, ty, tz) specifying rotation about the x, y, and z-axis and translation along the the x, y, and z-axis respectively
+ * \param[in] parameters (alpha, beta, gamma, tx, ty, tz) specifying rotation about the x, y, and z-axis and translation along the the x, y, and z-axis respectively
* \param[out] transformation_matrix the resultant transformation matrix
*/
inline void
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_representation.h>
#include <pcl/search/kdtree.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/eigen.h>
*/
#include <pcl/registration/correspondence_rejection_organized_boundary.h>
+#include <pcl/memory.h> // for static_pointer_cast
+
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
- pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud =
boost::static_pointer_cast<pcl::registration::DataContainer<pcl::PointXYZ, pcl::PointNormal> >(data_container_)->getInputTarget ();
+ pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud = static_pointer_cast<pcl::registration::DataContainer<pcl::PointXYZ, pcl::PointNormal> >(data_container_)->getInputTarget ();
if (!cloud->isOrganized ())
{
//////////////////////////////////////////////////////////////////////////////////////////////
float
-pcl::registration::CorrespondenceRejectorVarTrimmed::optimizeInlierRatio (std::vector <double>& dists)
+pcl::registration::CorrespondenceRejectorVarTrimmed::optimizeInlierRatio (std::vector <double>& dists) const
{
unsigned int points_nbr = static_cast<unsigned int> (dists.size ());
std::sort (dists.begin (), dists.end ());
#include <pcl/registration/gicp6d.h>
+#include <pcl/memory.h> // for pcl::make_shared
+
namespace pcl
{
// convert sRGB to CIELAB
// ...and build 6d-tree
target_tree_lab_.setInputCloud (target_lab_);
target_tree_lab_.setPointRepresentation (
- boost::make_shared<MyPointRepresentation> (point_rep_));
+ pcl::make_shared<MyPointRepresentation> (point_rep_));
}
bool
iterations_ = 0;
double d_best_penalty = std::numeric_limits<double>::max();
- std::vector<int> selection;
+ Indices selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
- int n_inliers_count = 0;
-
unsigned skipped_count = 0;
// suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Iterate
- while (iterations_ < max_iterations_ && skipped_count < max_skip)
+ while ((iterations_ < max_iterations_) && (skipped_count < max_skip))
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);
- if (selection.empty ()) break;
+ if (selection.empty ())
+ {
+ break;
+ }
// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
continue;
}
- double d_cur_penalty = 0;
+ double d_cur_penalty;
// d_cur_penalty = sum (min (dist, threshold))
// Iterate through the 3d points and calculate the distances from them to the model
++skipped_count;
continue;
}
+ // Move all NaNs in distances to the end
+ const auto new_end = (sac_model_->getInputCloud()->is_dense ? distances.end() : std::partition (distances.begin(), distances.end(), [](double d){return !std::isnan (d);}));
+ const auto nr_valid_dists = std::distance (distances.begin (), new_end);
- std::sort (distances.begin (), distances.end ());
// d_cur_penalty = median (distances)
- std::size_t mid = sac_model_->getIndices ()->size () / 2;
- if (mid >= distances.size ())
+ const std::size_t mid = nr_valid_dists / 2;
+ PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] There are %lu valid distances remaining after removing NaN values.\n", nr_valid_dists);
+ if (nr_valid_dists == 0)
{
//iterations_++;
++skipped_count;
}
// Do we have a "middle" point or should we "estimate" one ?
- if (sac_model_->getIndices ()->size () % 2 == 0)
- d_cur_penalty = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
+ if ((nr_valid_dists % 2) == 0)
+ {
+ // Looking at two values instead of one probably doesn't matter because they are mostly barely different, but let's do it for accuracy's sake
+ std::nth_element (distances.begin (), distances.begin () + (mid - 1), new_end);
+ const double tmp = distances[mid-1];
+ const double tmp2 = *(std::min_element (distances.begin () + mid, new_end));
+ d_cur_penalty = (sqrt (tmp) + sqrt (tmp2)) / 2.0;
+ PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with two values (%g and %g) because number of distances is even.\n", tmp, distances[mid]);
+ }
else
+ {
+ std::nth_element (distances.begin (), distances.begin () + mid, new_end);
d_cur_penalty = sqrt (distances[mid]);
+ PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with one value (%g) because number of distances is odd.\n", distances[mid]);
+ }
// Better match ?
if (d_cur_penalty < d_best_penalty)
++iterations_;
if (debug_verbosity_level > 1)
+ {
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, max_iterations_, d_best_penalty);
+ }
}
if (model_.empty ())
{
if (debug_verbosity_level > 0)
+ {
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Unable to find a solution!\n");
+ }
return (false);
}
return (false);
}
- std::vector<int> &indices = *sac_model_->getIndices ();
+ Indices &indices = *sac_model_->getIndices ();
if (distances.size () != indices.size ())
{
inliers_.resize (distances.size ());
// Get the inliers for the best model found
- n_inliers_count = 0;
+ std::size_t n_inliers_count = 0;
for (std::size_t i = 0; i < distances.size (); ++i)
+ {
if (distances[i] <= threshold_)
+ {
inliers_[n_inliers_count++] = indices[i];
+ }
+ }
// Resize the inliers vector
inliers_.resize (n_inliers_count);
if (debug_verbosity_level > 0)
- PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count);
+ {
+ PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %lu size, %lu inliers.\n", model_.size (), n_inliers_count);
+ }
return (true);
}
#define PCL_INSTANTIATE_LeastMedianSquares(T) template class PCL_EXPORTS pcl::LeastMedianSquares<T>;
#endif // PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_
-
double d_best_penalty = std::numeric_limits<double>::max();
double k = 1.0;
- std::vector<int> selection;
+ Indices selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
// Iterate through the 3d points and calculate the distances from them to the model again
sac_model_->getDistancesToModel (model_coefficients_, distances);
- std::vector<int> &indices = *sac_model_->getIndices ();
+ Indices &indices = *sac_model_->getIndices ();
if (distances.size () != indices.size ())
{
PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ());
double d_best_penalty = std::numeric_limits<double>::max();
double k = 1.0;
- std::vector<int> selection;
+ Indices selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
// Iterate through the 3d points and calculate the distances from them to the model again
sac_model_->getDistancesToModel (model_coefficients_, distances);
- std::vector<int> &indices = *sac_model_->getIndices ();
+ Indices &indices = *sac_model_->getIndices ();
if (distances.size () != indices.size ())
{
// Initialize the usual RANSAC parameters
iterations_ = 0;
- std::vector<int> inliers;
- std::vector<int> selection;
+ Indices inliers;
+ Indices selection;
Eigen::VectorXf model_coefficients;
// We will increase the pool so the indices_ vector can only contain m elements at first
- std::vector<int> index_pool;
+ Indices index_pool;
index_pool.reserve (N);
for (unsigned int i = 0; i < n; ++i)
index_pool.push_back (sac_model_->indices_->operator[](i));
// We only need to compute possible better epsilon_n_star for when _n is just about to be removed an inlier
std::size_t I_possible_n_star = I_N;
- for (std::vector<int>::const_reverse_iterator last_inlier = inliers.rbegin (),
- inliers_end = inliers.rend ();
+ for (auto last_inlier = inliers.crbegin (), inliers_end = inliers.crend ();
last_inlier != inliers_end;
++last_inlier, --I_possible_n_star)
{
std::size_t n_best_inliers_count = 0;
double k = std::numeric_limits<double>::max();
- std::vector<int> selection;
+ Indices selection;
Eigen::VectorXf model_coefficients;
const double log_probability = std::log (1.0 - probability_);
double d_best_penalty = std::numeric_limits<double>::max();
double k = 1.0;
- std::vector<int> selection;
+ Indices selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
- std::set<int> indices_subset;
+ std::set<index_t> indices_subset;
int n_inliers_count = 0;
unsigned skipped_count = 0;
// Iterate through the 3d points and calculate the distances from them to the model again
sac_model_->getDistancesToModel (model_coefficients_, distances);
- std::vector<int> &indices = *sac_model_->getIndices ();
+ Indices &indices = *sac_model_->getIndices ();
if (distances.size () != indices.size ())
{
PCL_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ());
std::size_t n_best_inliers_count = 0;
double k = std::numeric_limits<double>::max();
- std::vector<int> selection;
+ Indices selection;
Eigen::VectorXf model_coefficients;
- std::set<int> indices_subset;
+ std::set<index_t> indices_subset;
const double log_probability = std::log (1.0 - probability_);
const double one_over_indices = 1.0 / static_cast<double> (sac_model_->getIndices ()->size ());
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const std::vector<int> &samples) const
+pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const Indices &samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
// Get the values at the two points
Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
- if (samples.size () != 3)
+ if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
}
- model_coefficients.resize (3);
+ model_coefficients.resize (model_size_);
Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold,
- std::vector<int> &inliers)
+ Indices &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
inliers.clear ();
return;
}
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the circle
for (std::size_t i = 0; i < indices_->size (); ++i)
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = static_cast<double> (distance);
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (static_cast<double> (distance));
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
- if (model_coefficients.size () != 3)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Given model is invalid!\n");
return;
}
- // Need at least 3 samples
- if (inliers.size () <= 3)
+ // Need more than the minimum sample size to make a difference
+ if (inliers.size () <= sample_size_)
{
- PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
return;
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != 3)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Given model is invalid!\n");
return;
}
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the points and project them to the circle
- for (const int &inlier : inliers)
+ for (const auto &inlier : inliers)
{
float dx = input_->points[inlier].x - model_coefficients[0];
float dy = input_->points[inlier].y - model_coefficients[1];
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel (
- const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
+ const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid model coefficients
- if (model_coefficients.size () != 3)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
}
- for (const int &index : indices)
+ for (const auto &index : indices)
// Calculate the distance from the point to the circle as the difference between
//dist(point,circle_origin) and circle_radius
if (std::abs (std::sqrt (
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::isSampleGood (
- const std::vector<int> &samples) const
+ const Indices &samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
// Get the values at the three points
Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z);
Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
- if (samples.size () != 3)
+ if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
}
- model_coefficients.resize (7); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ
+ model_coefficients.resize (model_size_); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ
Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z);
Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z);
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold,
- std::vector<int> &inliers)
+ Indices &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
inliers.clear ();
return;
}
- int nr_p = 0;
- inliers.resize (indices_->size ());
+ inliers.clear ();
+ inliers.reserve (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the sphere
for (std::size_t i = 0; i < indices_->size (); ++i)
if (distanceVector.norm () < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- nr_p++;
+ inliers.push_back ((*indices_)[i]);
}
}
- inliers.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::optimizeModelCoefficients (
- const std::vector<int> &inliers,
+ const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Given model is invalid!\n");
return;
}
- // Need at least 3 samples
- if (inliers.size () <= 3)
+ // Need more than the minimum sample size to make a difference
+ if (inliers.size () <= sample_size_)
{
- PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
return;
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Given model is invalid!\n");
return;
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::doSamplesVerifyModel (
- const std::set<int> &indices,
+ const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const
{
// Needs a valid model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
}
- for (const int &index : indices)
+ for (const auto &index : indices)
{
// Calculate the distance from the point to the sphere as the difference between
//dist(point,sphere_origin) and sphere_radius
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
-pcl::SampleConsensusModelCone<PointT, PointNT>::isSampleGood(const std::vector<int> &) const
+pcl::SampleConsensusModelCone<PointT, PointNT>::isSampleGood(const Indices &samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelCone::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
- const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+ const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
- if (samples.size () != 3)
+ if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
return (false);
}
- Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
- Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);
- Eigen::Vector4f p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z, 0);
+ Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0.0f);
+ Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0.0f);
+ Eigen::Vector4f p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z, 0.0f);
- Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
- Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
- Eigen::Vector4f n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0);
+ Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0.0f);
+ Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0.0f);
+ Eigen::Vector4f n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0.0f);
//calculate apex (intersection of the three planes defined by points and belonging normals
Eigen::Vector4f ortho12 = n1.cross3(n2);
//compute opening angle
float opening_angle = ( std::acos (ap1.dot (axis_dir)) + std::acos (ap2.dot (axis_dir)) + std::acos (ap3.dot (axis_dir)) ) / 3.0f;
- model_coefficients.resize (7);
+ model_coefficients.resize (model_size_);
// model_coefficients.template head<3> () = line_pt.template head<3> ();
model_coefficients[0] = apex[0];
model_coefficients[1] = apex[1];
distances.resize (indices_->size ());
- Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (std::size_t i = 0; i < indices_->size (); ++i)
{
- Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
- Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
+ Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
+ Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
double d_normal = std::abs (getAngle3D (n, cone_normal));
d_normal = (std::min) (d_normal, M_PI - d_normal);
- distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
+ distances[i] = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return;
}
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
- Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (std::size_t i = 0; i < indices_->size (); ++i)
{
- Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
- Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
+ Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
+ Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
double d_normal = std::abs (getAngle3D (n, cone_normal));
d_normal = (std::min) (d_normal, M_PI - d_normal);
- double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
+ double distance = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = distance;
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (distance);
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
std::size_t nr_p = 0;
- Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (std::size_t i = 0; i < indices_->size (); ++i)
{
- Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
- Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
+ Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
+ Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
double d_normal = std::abs (getAngle3D (n, cone_normal));
d_normal = (std::min) (d_normal, M_PI - d_normal);
- if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
+ if (std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid) < threshold)
nr_p++;
}
return (nr_p);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::optimizeModelCoefficients (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Given model is invalid!\n");
return;
}
- if (inliers.empty ())
+ // Need more than the minimum sample size to make a difference
+ if (inliers.size () <= sample_size_)
{
- PCL_DEBUG ("[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n");
+ PCL_ERROR ("[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
return;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Given model is invalid!\n");
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
- Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cone
- for (const int &inlier : inliers)
+ for (const auto &inlier : inliers)
{
Eigen::Vector4f pt (input_->points[inlier].x,
input_->points[inlier].y,
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
- const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
+ const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
}
- Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float openning_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
- for (const int &index : indices)
+ for (const auto &index : indices)
{
- Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0);
+ Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0.0f);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
pcl::SampleConsensusModelCone<PointT, PointNT>::pointToAxisDistance (
const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
{
- Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
return sqrt(pcl::sqrPointToLineDistance (pt, apex, axis_dir));
}
if (eps_angle_ > 0.0)
{
// Obtain the cone direction
- Eigen::Vector4f coeff;
- coeff[0] = model_coefficients[3];
- coeff[1] = model_coefficients[4];
- coeff[2] = model_coefficients[5];
- coeff[3] = 0;
-
- Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
- double angle_diff = std::abs (getAngle3D (axis, coeff));
+ const Eigen::Vector3f coeff(model_coefficients[3], model_coefficients[4], model_coefficients[5]);
+
+ double angle_diff = std::abs (getAngle3D (axis_, coeff));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current cone model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
-pcl::SampleConsensusModelCylinder<PointT, PointNT>::isSampleGood (const std::vector<int> &) const
+pcl::SampleConsensusModelCylinder<PointT, PointNT>::isSampleGood (const Indices &samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
- const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+ const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 2 samples
- if (samples.size () != 2)
+ if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
return (false);
}
- Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
- Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);
+ Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0.0f);
+ Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0.0f);
- Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
- Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
+ Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0.0f);
+ Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0.0f);
Eigen::Vector4f w = n1 + p1 - p2;
float a = n1.dot (n1);
Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt;
line_dir.normalize ();
- model_coefficients.resize (7);
+ model_coefficients.resize (model_size_);
// model_coefficients.template head<3> () = line_pt.template head<3> ();
model_coefficients[0] = line_pt[0];
model_coefficients[1] = line_pt[1];
distances.resize (indices_->size ());
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float ptdotdir = line_pt.dot (line_dir);
float dirdotdir = 1.0f / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
// Approximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
// @note need to revise this.
- Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
- Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
+ Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
+ Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
double d_normal = std::abs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
- distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
+ distances[i] = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return;
}
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float ptdotdir = line_pt.dot (line_dir);
float dirdotdir = 1.0f / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
{
// Approximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
- Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
- Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
+ Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
+ Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
double d_normal = std::abs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
- double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
+ double distance = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = distance;
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (distance);
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
{
// Approximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
- Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
- Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
+ Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
+ Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
double d_normal = std::abs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
- if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
+ if (std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid) < threshold)
nr_p++;
}
return (nr_p);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::optimizeModelCoefficients (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Given model is invalid!\n");
return;
}
- if (inliers.empty ())
+ // Need more than the minimum sample size to make a difference
+ if (inliers.size () <= sample_size_)
{
- PCL_DEBUG ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n");
+ PCL_ERROR ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
return;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Given model is invalid!\n");
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float ptdotdir = line_pt.dot (line_dir);
float dirdotdir = 1.0f / line_dir.dot (line_dir);
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cylinder
- for (const int &inlier : inliers)
+ for (const auto &inlier : inliers)
{
Eigen::Vector4f p (input_->points[inlier].x,
input_->points[inlier].y,
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::doSamplesVerifyModel (
- const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
+ const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid model coefficients
- if (model_coefficients.size () != 7)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
}
- for (const int &index : indices)
+ for (const auto &index : indices)
{
// Approximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
// @note need to revise this.
- Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0);
+ Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0.0f);
if (std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold)
return (false);
}
pcl::SampleConsensusModelCylinder<PointT, PointNT>::pointToLineDistance (
const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
{
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
return sqrt(pcl::sqrPointToLineDistance (pt, line_pt, line_dir));
}
pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPointToCylinder (
const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const
{
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir);
pt_proj = line_pt + k * line_dir;
if (eps_angle_ > 0.0)
{
// Obtain the cylinder direction
- Eigen::Vector4f coeff;
- coeff[0] = model_coefficients[3];
- coeff[1] = model_coefficients[4];
- coeff[2] = model_coefficients[5];
- coeff[3] = 0;
-
- Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
- double angle_diff = std::abs (getAngle3D (axis, coeff));
+ const Eigen::Vector3f coeff(model_coefficients[3], model_coefficients[4], model_coefficients[5]);
+
+ double angle_diff = std::abs (getAngle3D (axis_, coeff));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current cylinder model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelLine<PointT>::isSampleGood (const std::vector<int> &samples) const
+pcl::SampleConsensusModelLine<PointT>::isSampleGood (const Indices &samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelLine::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
// Make sure that the two sample points are not identical
if (
(input_->points[samples[0]].x != input_->points[samples[1]].x)
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
- const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+ const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 2 samples
- if (samples.size () != 2)
+ if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
return (false);
}
- model_coefficients.resize (6);
+ model_coefficients.resize (model_size_);
model_coefficients[0] = input_->points[samples[0]].x;
model_coefficients[1] = input_->points[samples[0]].y;
model_coefficients[2] = input_->points[samples[0]].z;
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
+ {
return;
+ }
distances.resize (indices_->size ());
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
double sqr_threshold = threshold * threshold;
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
// Obtain the line point and direction
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
if (sqr_distance < sqr_threshold)
{
// Returns the indices of the points whose squared distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = sqr_distance;
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (sqr_distance);
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////
std::size_t nr_p = 0;
// Obtain the line point and direction
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
line_dir.normalize ();
// Iterate through the 3d points and calculate the distances from them to the line
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return;
}
- // Need at least 2 points to estimate a line
- if (inliers.size () <= 2)
+ // Need more than the minimum sample size to make a difference
+ if (inliers.size () <= sample_size_)
{
- PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
- optimized_coefficients.resize (6);
+ optimized_coefficients.resize (model_size_);
// Compute the 3x3 covariance matrix
Eigen::Vector4f centroid;
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::projectPoints (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid model coefficients
if (!isModelValid (model_coefficients))
return;
// Obtain the line point and direction
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the line
- for (const int &inlier : inliers)
+ for (const auto &inlier : inliers)
{
- Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0);
+ Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0.0f);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the line
for (std::size_t i = 0; i < inliers.size (); ++i)
{
- Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
+ Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0.0f);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::doSamplesVerifyModel (
- const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
+ const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return (false);
// Obtain the line point and direction
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
line_dir.normalize ();
double sqr_threshold = threshold * threshold;
// Iterate through the 3d points and calculate the distances from them to the line
- for (const int &index : indices)
+ for (const auto &index : indices)
{
// Calculate the distance from the point to the line
// D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
{
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
- coeff[3] = 0;
+ coeff[3] = 0.0f;
coeff.normalize ();
if (std::abs (axis_.dot (coeff)) < cos_angle_)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
if (!normals_)
{
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
- coeff[3] = 0;
+ coeff[3] = 0.0f;
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (std::size_t i = 0; i < indices_->size (); ++i)
const PointNT &nt = normals_->points[(*indices_)[i]];
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
- Eigen::Vector4f p (pt.x, pt.y, pt.z, 0);
- Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0);
+ Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
+ Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
// Calculate the angular distance between the point normal and the plane normal
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = distance;
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (distance);
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
- coeff[3] = 0;
+ coeff[3] = 0.0f;
std::size_t nr_p = 0;
const PointNT &nt = normals_->points[(*indices_)[i]];
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
- Eigen::Vector4f p (pt.x, pt.y, pt.z, 0);
- Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0);
+ Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
+ Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
// Calculate the angular distance between the point normal and the plane normal
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
- coeff[3] = 0;
+ coeff[3] = 0.0f;
distances.resize (indices_->size ());
const PointNT &nt = normals_->points[(*indices_)[i]];
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
- Eigen::Vector4f p (pt.x, pt.y, pt.z, 0);
- Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0);
+ Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
+ Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
// Calculate the angular distance between the point normal and the plane normal
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
if (!normals_)
{
// Obtain the sphere center
Eigen::Vector4f center = model_coefficients;
- center[3] = 0;
+ center[3] = 0.0f;
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the sphere
for (std::size_t i = 0; i < indices_->size (); ++i)
Eigen::Vector4f p (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
- 0);
+ 0.0f);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2],
- 0);
+ 0.0f);
Eigen::Vector4f n_dir = p - center;
double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
double d_normal = std::abs (getAngle3D (n, n_dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
- double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
+ double distance = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = static_cast<double> (distance);
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (static_cast<double> (distance));
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Obtain the sphere centroid
Eigen::Vector4f center = model_coefficients;
- center[3] = 0;
+ center[3] = 0.0f;
std::size_t nr_p = 0;
Eigen::Vector4f p (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
- 0);
+ 0.0f);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2],
- 0);
+ 0.0f);
Eigen::Vector4f n_dir = (p-center);
double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
double d_normal = std::abs (getAngle3D (n, n_dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
- if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
+ if (std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid) < threshold)
nr_p++;
}
return (nr_p);
// Obtain the sphere centroid
Eigen::Vector4f center = model_coefficients;
- center[3] = 0;
+ center[3] = 0.0f;
distances.resize (indices_->size ());
Eigen::Vector4f p (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
- 0);
+ 0.0f);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2],
- 0);
+ 0.0f);
Eigen::Vector4f n_dir = (p-center);
double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename PointNT> bool
-pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
-{
- if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
- return (false);
-
- if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_)
- return (false);
- if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_)
- return (false);
-
- return (true);
-}
-
#define PCL_INSTANTIATE_SampleConsensusModelNormalSphere(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalSphere<PointT, PointNT>;
#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelParallelLine<PointT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
+ {
return (0);
+ }
return (SampleConsensusModelLine<PointT>::countWithinDistance (model_coefficients, threshold));
}
if (eps_angle_ > 0.0)
{
// Obtain the line direction
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
- Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
+ Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0.0f);
double angle_diff = std::abs (getAngle3D (axis, line_dir));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current line model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
+ {
return (false);
+ }
}
return (true);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelParallelPlane<PointT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
+ {
return (0);
+ }
return (SampleConsensusModelPlane<PointT>::countWithinDistance (model_coefficients, threshold));
}
pcl::SampleConsensusModelParallelPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
+ {
return (false);
+ }
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
- coeff[3] = 0;
+ coeff[3] = 0.0f;
coeff.normalize ();
- Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
+ Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0.0f);
if (std::abs (axis.dot (coeff)) > sin_angle_)
+ {
return (false);
+ }
}
return (true);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPerpendicularPlane<PointT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
+ {
return (0);
+ }
return (SampleConsensusModelPlane<PointT>::countWithinDistance (model_coefficients, threshold));
}
pcl::SampleConsensusModelPerpendicularPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
+ {
return (false);
+ }
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
- coeff[3] = 0;
+ coeff[3] = 0.0f;
- Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
+ Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0.0f);
double angle_diff = std::abs (getAngle3D (axis, coeff));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
+ {
return (false);
+ }
}
return (true);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelPlane<PointT>::isSampleGood (const std::vector<int> &samples) const
+pcl::SampleConsensusModelPlane<PointT>::isSampleGood (const Indices &samples) const
{
- // Need an extra check in case the sample selection is empty
- if (samples.empty ())
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelPlane::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
return (false);
+ }
// Get the values at the two points
pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap ();
pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap ();
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients (
- const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+ const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
if (samples.size () != sample_size_)
// Avoid some crashes by checking for collinearity here
Eigen::Array4f dy1dy2 = p1p0 / p2p0;
if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) ) // Check for collinearity
+ {
return (false);
+ }
// Compute the plane coefficients from the 3 given points in a straightforward manner
// calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
- model_coefficients.resize (4);
+ model_coefficients.resize (model_size_);
model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
- model_coefficients[3] = 0;
+ model_coefficients[3] = 0.0f;
// Normalize
model_coefficients.normalize ();
// ... + d = 0
- model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ()));
+ model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));
return (true);
}
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != model_size_)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Given model is invalid!\n");
return;
}
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
- 1);
+ 1.0f);
distances[i] = std::abs (model_coefficients.dot (pt));
}
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != model_size_)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n");
return;
}
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (std::size_t i = 0; i < indices_->size (); ++i)
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
- 1);
+ 1.0f);
float distance = std::abs (model_coefficients.dot (pt));
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = static_cast<double> (distance);
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (static_cast<double> (distance));
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////
const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != model_size_)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
return (0);
}
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
- 1);
+ 1.0f);
if (std::abs (model_coefficients.dot (pt)) < threshold)
+ {
nr_p++;
+ }
}
return (nr_p);
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != model_size_)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n");
optimized_coefficients = model_coefficients;
return;
}
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
// Hessian form (D = nc . p_plane (centroid here) + p)
- optimized_coefficients.resize (4);
+ optimized_coefficients.resize (model_size_);
optimized_coefficients[0] = eigen_vector [0];
optimized_coefficients[1] = eigen_vector [1];
optimized_coefficients[2] = eigen_vector [2];
- optimized_coefficients[3] = 0;
- optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid);
+ optimized_coefficients[3] = 0.0f;
+ optimized_coefficients[3] = -1.0f * optimized_coefficients.dot (xyz_centroid);
// Make sure it results in a valid model
if (!isModelValid (optimized_coefficients))
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != model_size_)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n");
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
- Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
+ Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
// normalize the vector perpendicular to the plane...
mc.normalize ();
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
- for (const int &inlier : inliers)
+ for (const auto &inlier : inliers)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inlier].x,
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Iterate over each point
for (std::size_t i = 0; i < inliers.size (); ++i)
+ {
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
+ }
// Iterate through the 3d points and calculate the distances from them to the plane
for (std::size_t i = 0; i < inliers.size (); ++i)
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
- 1);
+ 1.0f);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::doSamplesVerifyModel (
- const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
+ const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid set of model coefficients
- if (model_coefficients.size () != model_size_)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
}
- for (const int &index : indices)
+ for (const auto &index : indices)
{
Eigen::Vector4f pt (input_->points[index].x,
input_->points[index].y,
input_->points[index].z,
- 1);
+ 1.0f);
if (std::abs (model_coefficients.dot (pt)) > threshold)
+ {
return (false);
+ }
}
return (true);
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
#include <pcl/sample_consensus/sac_model_registration.h>
-#include <pcl/common/point_operators.h>
#include <pcl/common/eigen.h>
#include <pcl/point_types.h>
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelRegistration<PointT>::isSampleGood (const std::vector<int> &samples) const
+pcl::SampleConsensusModelRegistration<PointT>::isSampleGood (const Indices &samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelRegistration::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
using namespace pcl::common;
using namespace pcl::traits;
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
if (!target_)
{
return (false);
}
// Need 3 samples
- if (samples.size () != 3)
+ if (samples.size () != sample_size_)
+ {
return (false);
+ }
- std::vector<int> indices_tgt (3);
+ Indices indices_tgt (3);
for (int i = 0; i < 3; ++i)
+ {
indices_tgt[i] = correspondences_.at (samples[i]);
+ }
estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
return (true);
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
- input_->points[(*indices_)[i]].z, 1);
+ input_->points[(*indices_)[i]].z, 1.0f);
Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
target_->points[(*indices_tgt_)[i]].y,
- target_->points[(*indices_tgt_)[i]].z, 1);
+ target_->points[(*indices_tgt_)[i]].z, 1.0f);
Eigen::Vector4f p_tr (transform * pt_src);
// Calculate the distance from the transformed point to its correspondence
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
-pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
if (indices_->size () != indices_tgt_->size ())
{
return;
}
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
Eigen::Matrix4f transform;
transform.row (0).matrix () = model_coefficients.segment<4>(0);
// Calculate the distance from the transformed point to its correspondence
if (distance < thresh)
{
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = static_cast<double> (distance);
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (static_cast<double> (distance));
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
+ {
return (0);
+ }
Eigen::Matrix4f transform;
transform.row (0).matrix () = model_coefficients.segment<4>(0);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
-pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
+pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
if (indices_->size () != indices_tgt_->size ())
{
return;
}
- std::vector<int> indices_src (inliers.size ());
- std::vector<int> indices_tgt (inliers.size ());
+ Indices indices_src (inliers.size ());
+ Indices indices_tgt (inliers.size ());
for (std::size_t i = 0; i < inliers.size (); ++i)
{
indices_src[i] = inliers[i];
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
const pcl::PointCloud<PointT> &cloud_src,
- const std::vector<int> &indices_src,
+ const Indices &indices_src,
const pcl::PointCloud<PointT> &cloud_tgt,
- const std::vector<int> &indices_tgt,
+ const Indices &indices_tgt,
Eigen::VectorXf &transform) const
{
transform.resize (16);
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
#include <pcl/sample_consensus/sac_model_registration_2d.h>
-#include <pcl/common/point_operators.h>
#include <pcl/common/eigen.h>
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelRegistration2D<PointT>::isSampleGood (const std::vector<int>&) const
+pcl::SampleConsensusModelRegistration2D<PointT>::isSampleGood (const Indices&samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
return (true);
//using namespace pcl::common;
//using namespace pcl::traits;
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
- input_->points[(*indices_)[i]].z, 1);
+ input_->points[(*indices_)[i]].z, 1.0f);
Eigen::Vector4f p_tr (transform * pt_src);
Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
Eigen::Vector3f uv (projection_matrix_ * p_tr3);
- if (uv[2] < 0)
+ if (uv[2] < 0.0f)
+ {
continue;
+ }
uv /= uv[2];
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
-pcl::SampleConsensusModelRegistration2D<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+pcl::SampleConsensusModelRegistration2D<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
if (indices_->size () != indices_tgt_->size ())
{
double thresh = threshold * threshold;
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
Eigen::Matrix4f transform;
transform.row (0).matrix () = model_coefficients.segment<4>(0);
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
- input_->points[(*indices_)[i]].z, 1);
+ input_->points[(*indices_)[i]].z, 1.0f);
Eigen::Vector4f p_tr (transform * pt_src);
Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
Eigen::Vector3f uv (projection_matrix_ * p_tr3);
- if (uv[2] < 0)
+ if (uv[2] < 0.0f)
continue;
uv /= uv[2];
// Calculate the distance from the transformed point to its correspondence
if (distance < thresh)
{
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = distance;
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (distance);
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
- input_->points[(*indices_)[i]].z, 1);
+ input_->points[(*indices_)[i]].z, 1.0f);
Eigen::Vector4f p_tr (transform * pt_src);
Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
Eigen::Vector3f uv (projection_matrix_ * p_tr3);
- if (uv[2] < 0)
+ if (uv[2] < 0.0f)
+ {
continue;
+ }
uv /= uv[2];
(uv[0] - target_->points[(*indices_tgt_)[i]].u) +
(uv[1] - target_->points[(*indices_tgt_)[i]].v) *
(uv[1] - target_->points[(*indices_tgt_)[i]].v)) < thresh)
+ {
++nr_p;
+ }
}
return (nr_p);
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelSphere<PointT>::isSampleGood (const std::vector<int> &) const
+pcl::SampleConsensusModelSphere<PointT>::isSampleGood (const Indices &samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelSphere::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
return (true);
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
- const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+ const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 4 samples
- if (samples.size () != 4)
+ if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
}
float m11 = temp.determinant ();
if (m11 == 0)
+ {
return (false); // the points don't define a sphere!
+ }
for (int i = 0; i < 4; ++i)
+ {
temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
(input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
(input_->points[samples[i]].z) * (input_->points[samples[i]].z);
+ }
float m12 = temp.determinant ();
for (int i = 0; i < 4; ++i)
float m15 = temp.determinant ();
// Center (x , y, z)
- model_coefficients.resize (4);
+ model_coefficients.resize (model_size_);
model_coefficients[0] = 0.5f * m12 / m11;
model_coefficients[1] = 0.5f * m13 / m11;
model_coefficients[2] = 0.5f * m14 / m11;
// Iterate through the 3d points and calculate the distances from them to the sphere
for (std::size_t i = 0; i < indices_->size (); ++i)
+ {
// Calculate the distance from the point to the sphere as the difference between
//dist(point,sphere_origin) and sphere_radius
distances[i] = std::abs (std::sqrt (
( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
( input_->points[(*indices_)[i]].z - model_coefficients[2] )
) - model_coefficients[3]);
+ }
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelSphere<PointT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return;
}
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the sphere
for (std::size_t i = 0; i < indices_->size (); ++i)
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = static_cast<double> (distance);
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (static_cast<double> (distance));
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelSphere<PointT>::optimizeModelCoefficients (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
- if (model_coefficients.size () != 4)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Given model is invalid!\n");
return;
}
- // Need at least 4 samples
- if (inliers.size () <= 4)
+ // Need more than the minimum sample size to make a difference
+ if (inliers.size () <= sample_size_)
{
- PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
return;
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelSphere<PointT>::projectPoints (
- const std::vector<int> &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const
+ const Indices &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const
{
// Needs a valid model coefficients
- if (model_coefficients.size () != 4)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Given model is invalid!\n");
return;
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelSphere<PointT>::doSamplesVerifyModel (
- const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
+ const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid model coefficients
- if (model_coefficients.size () != 4)
+ if (!isModelValid (model_coefficients))
{
- PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
}
- for (const int &index : indices)
+ for (const auto &index : indices)
+ {
// Calculate the distance from the point to the sphere as the difference between
//dist(point,sphere_origin) and sphere_radius
if (std::abs (sqrt (
( input_->points[index].z - model_coefficients[2] ) *
( input_->points[index].z - model_coefficients[2] )
) - model_coefficients[3]) > threshold)
+ {
return (false);
+ }
+ }
return (true);
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
-pcl::SampleConsensusModelStick<PointT>::isSampleGood (const std::vector<int> &samples) const
+pcl::SampleConsensusModelStick<PointT>::isSampleGood (const Indices &samples) const
{
+ if (samples.size () != sample_size_)
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelStick::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
+ return (false);
+ }
if (
(input_->points[samples[0]].x != input_->points[samples[1]].x)
&&
(input_->points[samples[0]].y != input_->points[samples[1]].y)
&&
(input_->points[samples[0]].z != input_->points[samples[1]].z))
+ {
return (true);
+ }
return (false);
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelStick<PointT>::computeModelCoefficients (
- const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
+ const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 2 samples
- if (samples.size () != 2)
+ if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
}
- model_coefficients.resize (7);
+ model_coefficients.resize (model_size_);
model_coefficients[0] = input_->points[samples[0]].x;
model_coefficients[1] = input_->points[samples[0]].y;
model_coefficients[2] = input_->points[samples[0]].z;
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelStick::getDistancesToModel] Given model is invalid!\n");
return;
+ }
float sqr_threshold = static_cast<float> (radius_max_ * radius_max_);
distances.resize (indices_->size ());
// Obtain the line point and direction
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
line_dir.normalize ();
// Iterate through the 3d points and calculate the distances from them to the line
float sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
if (sqr_distance < sqr_threshold)
+ {
// Need to estimate sqrt here to keep MSAC and friends general
distances[i] = sqrt (sqr_distance);
+ }
else
+ {
// Penalize outliers by doubling the distance
distances[i] = 2 * sqrt (sqr_distance);
+ }
}
}
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::selectWithinDistance (
- const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
+ const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelStick::selectWithinDistance] Given model is invalid!\n");
return;
+ }
float sqr_threshold = static_cast<float> (threshold * threshold);
- int nr_p = 0;
- inliers.resize (indices_->size ());
- error_sqr_dists_.resize (indices_->size ());
+ inliers.clear ();
+ error_sqr_dists_.clear ();
+ inliers.reserve (indices_->size ());
+ error_sqr_dists_.reserve (indices_->size ());
// Obtain the line point and direction
- Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
Eigen::Vector4f line_dir = line_pt2 - line_pt1;
//Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
//Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
if (sqr_distance < sqr_threshold)
{
// Returns the indices of the points whose squared distances are smaller than the threshold
- inliers[nr_p] = (*indices_)[i];
- error_sqr_dists_[nr_p] = static_cast<double> (sqr_distance);
- ++nr_p;
+ inliers.push_back ((*indices_)[i]);
+ error_sqr_dists_.push_back (static_cast<double> (sqr_distance));
}
}
- inliers.resize (nr_p);
- error_sqr_dists_.resize (nr_p);
}
///////////////////////////////////////////////////////////////////////////
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelStick::countWithinDistance] Given model is invalid!\n");
return (0);
+ }
float sqr_threshold = static_cast<float> (threshold * threshold);
std::size_t nr_i = 0, nr_o = 0;
// Obtain the line point and direction
- Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
Eigen::Vector4f line_dir = line_pt2 - line_pt1;
line_dir.normalize ();
float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
// Use a larger threshold (4 times the radius) to get more points in
if (sqr_distance < sqr_threshold)
+ {
nr_i++;
- else if (sqr_distance < 4 * sqr_threshold)
+ }
+ else if (sqr_distance < 4.0f * sqr_threshold)
+ {
nr_o++;
+ }
}
return (nr_i <= nr_o ? 0 : nr_i - nr_o);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::optimizeModelCoefficients (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return;
}
- // Need at least 2 points to estimate a line
- if (inliers.size () <= 2)
+ // Need more than the minimum sample size to make a difference
+ if (inliers.size () <= sample_size_)
{
- PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
+ PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
- optimized_coefficients.resize (7);
+ optimized_coefficients.resize (model_size_);
// Compute the 3x3 covariance matrix
Eigen::Vector4f centroid;
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::projectPoints (
- const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
+ const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid model coefficients
if (!isModelValid (model_coefficients))
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelStick::projectPoints] Given model is invalid!\n");
return;
+ }
// Obtain the line point and direction
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Iterate over each point
for (std::size_t i = 0; i < projected_points.points.size (); ++i)
+ {
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
+ }
// Iterate through the 3d points and calculate the distances from them to the line
- for (const int &inlier : inliers)
+ for (const auto &inlier : inliers)
{
- Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0);
+ Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0.0f);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Iterate over each point
for (std::size_t i = 0; i < inliers.size (); ++i)
+ {
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
+ }
// Iterate through the 3d points and calculate the distances from them to the line
for (std::size_t i = 0; i < inliers.size (); ++i)
{
- Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
+ Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0.0f);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelStick<PointT>::doSamplesVerifyModel (
- const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
+ const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
+ {
+ PCL_ERROR ("[pcl::SampleConsensusModelStick::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
+ }
// Obtain the line point and direction
- Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
- Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
+ Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
+ Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0.0f);
//Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
line_dir.normalize ();
float sqr_threshold = static_cast<float> (threshold * threshold);
// Iterate through the 3d points and calculate the distances from them to the line
- for (const int &index : indices)
+ for (const auto &index : indices)
{
// Calculate the distance from the point to the line
// D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
if ((line_pt - input_->points[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
+ {
return (false);
+ }
}
return (true);
double sigma_sqr = sigma * sigma;
unsigned int refine_iterations = 0;
bool inlier_changed = false, oscillating = false;
- std::vector<int> new_inliers, prev_inliers = inliers_;
+ Indices new_inliers, prev_inliers = inliers_;
std::vector<std::size_t> inliers_sizes;
Eigen::VectorXf new_model_coefficients = model_coefficients_;
do
inline void
getRandomSamples (const IndicesPtr &indices,
std::size_t nr_samples,
- std::set<int> &indices_subset)
+ std::set<index_t> &indices_subset)
{
indices_subset.clear ();
while (indices_subset.size () < nr_samples)
- //indices_subset.insert ((*indices)[(int) (indices->size () * (rand () / (RAND_MAX + 1.0)))]);
- indices_subset.insert ((*indices)[static_cast<int> (static_cast<double>(indices->size ()) * rnd ())]);
+ //indices_subset.insert ((*indices)[(index_t) (indices->size () * (rand () / (RAND_MAX + 1.0)))]);
+ indices_subset.insert ((*indices)[static_cast<index_t> (static_cast<double>(indices->size ()) * rnd ())]);
}
/** \brief Return the best model found so far.
* \param[out] model the resultant model
*/
inline void
- getModel (std::vector<int> &model) const { model = model_; }
+ getModel (Indices &model) const { model = model_; }
/** \brief Return the best set of inliers found so far for this model.
* \param[out] inliers the resultant set of inliers
*/
inline void
- getInliers (std::vector<int> &inliers) const { inliers = inliers_; }
+ getInliers (Indices &inliers) const { inliers = inliers_; }
/** \brief Return the model coefficients of the best model found so far.
* \param[out] model_coefficients the resultant model coefficients, as documented in \ref sample_consensus
SampleConsensusModelPtr sac_model_;
/** \brief The model found after the last computeModel () as point cloud indices. */
- std::vector<int> model_;
+ Indices model_;
/** \brief The indices of the points that were chosen as inliers after the last computeModel () call. */
- std::vector<int> inliers_;
+ Indices inliers_;
/** \brief The coefficients of our model computed directly from the model found. */
Eigen::VectorXf model_coefficients_;
#include <memory>
#include <set>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/pcl_base.h>
#include <pcl/console/print.h>
#include <pcl/point_cloud.h>
+#include <pcl/types.h> // for index_t, Indices
#include <pcl/sample_consensus/boost.h>
#include <pcl/sample_consensus/model_types.h>
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModel (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: input_ (cloud)
- , indices_ (new std::vector<int> (indices))
+ , indices_ (new Indices (indices))
, radius_min_ (-std::numeric_limits<double>::max ())
, radius_max_ (std::numeric_limits<double>::max ())
, samples_radius_ (0.)
* \param[out] samples the resultant model samples
*/
virtual void
- getSamples (int &iterations, std::vector<int> &samples)
+ getSamples (int &iterations, Indices &samples)
{
// We're assuming that indices_ have already been set in the constructor
if (indices_->size () < getSampleSize ())
* \param[out] model_coefficients the computed model coefficients
*/
virtual bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const = 0;
/** \brief Recompute the model coefficients using the given inlier set
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
*/
virtual void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const = 0;
virtual void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) = 0;
+ Indices &inliers) = 0;
/** \brief Count all the points which respect the given model
* coefficients as inliers. Pure virtual.
* the point projections on the plane model
*/
virtual void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const = 0;
* determining the inliers from the outliers
*/
virtual bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const = 0;
{
input_ = cloud;
if (!indices_)
- indices_.reset (new std::vector<int> ());
+ indices_.reset (new Indices ());
if (indices_->empty ())
{
// Prepare a set of indices to be used (entire cloud)
indices_->resize (cloud->points.size ());
for (std::size_t i = 0; i < cloud->points.size (); ++i)
- (*indices_)[i] = static_cast<int> (i);
+ (*indices_)[i] = static_cast<index_t> (i);
}
shuffled_indices_ = *indices_;
}
* \param[out] indices the vector of indices that represents the input data.
*/
inline void
- setIndices (const std::vector<int> &indices)
+ setIndices (const Indices &indices)
{
- indices_.reset (new std::vector<int> (indices));
+ indices_.reset (new Indices (indices));
shuffled_indices_ = indices;
}
* \param[out] sample the set of indices of target_ to analyze
*/
inline void
- drawIndexSample (std::vector<int> &sample)
+ drawIndexSample (Indices &sample)
{
std::size_t sample_size = sample.size ();
std::size_t index_size = shuffled_indices_.size ();
* \param[out] sample the set of indices of target_ to analyze
*/
inline void
- drawIndexSampleRadius (std::vector<int> &sample)
+ drawIndexSampleRadius (Indices &sample)
{
std::size_t sample_size = sample.size ();
std::size_t index_size = shuffled_indices_.size ();
std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]);
//const PointT& pt0 = (*input_)[shuffled_indices_[0]];
- std::vector<int> indices;
+ Indices indices;
std::vector<float> sqr_dists;
// If indices have been set when the search object was constructed,
{
if (model_coefficients.size () != model_size_)
{
- PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (%lu)!\n", getClassName ().c_str (), model_coefficients.size ());
+ PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (is %lu, should be %lu)!\n", getClassName ().c_str (), model_coefficients.size (), model_size_);
return (false);
}
return (true);
* \param[in] samples the resultant index samples
*/
virtual bool
- isSampleGood (const std::vector<int> &samples) const = 0;
+ isSampleGood (const Indices &samples) const = 0;
/** \brief The model name. */
std::string model_name_;
SearchPtr samples_radius_search_;
/** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */
- std::vector<int> shuffled_indices_;
+ Indices shuffled_indices_;
/** \brief Boost-based random number generator algorithm. */
boost::mt19937 rng_alg_;
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
{
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all distances from the cloud data to a given 2D circle model.
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
* \param[in] copy_data_fields set to true if we need to copy the other data fields
*/
void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const override;
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const override;
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood(const std::vector<int> &samples) const override;
+ isSampleGood(const Indices &samples) const override;
private:
/** \brief Functor for the optimization function */
* \param[in] indices the indices of data points to evaluate
* \param[in] estimator pointer to the estimator object
*/
- OptimizationFunctor (const pcl::SampleConsensusModelCircle2D<PointT> *model, const std::vector<int>& indices) :
+ OptimizationFunctor (const pcl::SampleConsensusModelCircle2D<PointT> *model, const Indices& indices) :
pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
/** Cost function to be minimized
}
const pcl::SampleConsensusModelCircle2D<PointT> *model_;
- const std::vector<int> &indices_;
+ const Indices &indices_;
};
};
}
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelCircle3D (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
{
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all distances from the cloud data to a given 3D circle model.
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
* \param[in] copy_data_fields set to true if we need to copy the other data fields
*/
void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const override;
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const override;
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood(const std::vector<int> &samples) const override;
+ isSampleGood(const Indices &samples) const override;
private:
/** \brief Functor for the optimization function */
* \param[in] indices the indices of data points to evaluate
* \param[in] estimator pointer to the estimator object
*/
- OptimizationFunctor (const pcl::SampleConsensusModelCircle3D<PointT> *model, const std::vector<int>& indices) :
+ OptimizationFunctor (const pcl::SampleConsensusModelCircle3D<PointT> *model, const Indices& indices) :
pcl::Functor<double> (indices.size ()), model_ (model), indices_ (indices) {}
/** Cost function to be minimized
}
const pcl::SampleConsensusModelCircle3D<PointT> *model_;
- const std::vector<int> &indices_;
+ const Indices &indices_;
};
};
}
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelCone (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all distances from the cloud data to a given cone model.
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
* \param[in] copy_data_fields set to true if we need to copy the other data fields
*/
void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const override;
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const override;
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood (const std::vector<int> &samples) const override;
+ isSampleGood (const Indices &samples) const override;
private:
/** \brief The axis along which we need to search for a cone direction. */
* \param[in] indices the indices of data points to evaluate
* \param[in] estimator pointer to the estimator object
*/
- OptimizationFunctor (const pcl::SampleConsensusModelCone<PointT, PointNT> *model, const std::vector<int>& indices) :
+ OptimizationFunctor (const pcl::SampleConsensusModelCone<PointT, PointNT> *model, const Indices& indices) :
pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
/** Cost function to be minimized
}
const pcl::SampleConsensusModelCone<PointT, PointNT> *model_;
- const std::vector<int> &indices_;
+ const Indices &indices_;
};
};
}
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelCylinder (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all distances from the cloud data to a given cylinder model.
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
* \param[in] copy_data_fields set to true if we need to copy the other data fields
*/
void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const override;
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const override;
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood (const std::vector<int> &samples) const override;
+ isSampleGood (const Indices &samples) const override;
private:
/** \brief The axis along which we need to search for a cylinder direction. */
* \param[in] indices the indices of data points to evaluate
* \param[in] estimator pointer to the estimator object
*/
- OptimizationFunctor (const pcl::SampleConsensusModelCylinder<PointT, PointNT> *model, const std::vector<int>& indices) :
+ OptimizationFunctor (const pcl::SampleConsensusModelCylinder<PointT, PointNT> *model, const Indices& indices) :
pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
/** Cost function to be minimized
}
const pcl::SampleConsensusModelCylinder<PointT, PointNT> *model_;
- const std::vector<int> &indices_;
+ const Indices &indices_;
};
};
}
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelLine (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
{
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all squared distances from the cloud data to a given line model.
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed coefficients after optimization
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
* \param[in] copy_data_fields set to true if we need to copy the other data fields
*/
void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const override;
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const override;
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood (const std::vector<int> &samples) const override;
+ isSampleGood (const Indices &samples) const override;
};
}
#include <pcl/sample_consensus/sac_model_normal_plane.h>
#include <pcl/sample_consensus/model_types.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModelNormalPlane<PointT, PointNT> (cloud, indices, random)
, axis_ (Eigen::Vector4f::Zero ())
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/sample_consensus/sac_model.h>
#include <pcl/sample_consensus/sac_model_plane.h>
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelNormalPlane (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModelPlane<PointT> (cloud, indices, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/common/common.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelNormalSphere (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModelSphere<PointT> (cloud, indices, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
* \param[in] model_coefficients the coefficients of a model that we need to compute distances to
protected:
using SampleConsensusModel<PointT>::sample_size_;
using SampleConsensusModel<PointT>::model_size_;
-
- /** \brief Check whether a model is valid given the user constraints.
- * \param[in] model_coefficients the set of model coefficients
- */
- bool
- isModelValid (const Eigen::VectorXf &model_coefficients) const override;
-
+ using SampleConsensusModelSphere<PointT>::isModelValid;
};
}
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModelLine<PointT> (cloud, indices, random)
, axis_ (Eigen::Vector3f::Zero ())
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelParallelPlane (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModelPlane<PointT> (cloud, indices, random)
, axis_ (Eigen::Vector3f::Zero ())
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelPerpendicularPlane (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModelPlane<PointT> (cloud, indices, random)
, axis_ (Eigen::Vector3f::Zero ())
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelPlane (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
{
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all distances from the cloud data to a given plane model.
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
* \param[in] copy_data_fields set to true if we need to copy the other data fields
*/
void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const override;
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const override;
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood (const std::vector<int> &samples) const override;
+ isSampleGood (const Indices &samples) const override;
};
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/pcl_base.h>
#include <pcl/sample_consensus/eigen.h>
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelRegistration (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
, target_ ()
setInputTarget (const PointCloudConstPtr &target)
{
target_ = target;
- indices_tgt_.reset (new std::vector<int>);
+ indices_tgt_.reset (new Indices);
// Cache the size and fill the target indices
int target_size = static_cast<int> (target->size ());
indices_tgt_->resize (target_size);
* \param[in] indices_tgt a vector of point indices to be used from \a target
*/
inline void
- setInputTarget (const PointCloudConstPtr &target, const std::vector<int> &indices_tgt)
+ setInputTarget (const PointCloudConstPtr &target, const Indices &indices_tgt)
{
target_ = target;
- indices_tgt_.reset (new std::vector<int> (indices_tgt));
+ indices_tgt_.reset (new Indices (indices_tgt));
computeOriginalIndexMapping ();
}
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all distances from the transformed points to their correspondences
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed transformation
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
void
- projectPoints (const std::vector<int> &,
+ projectPoints (const Indices &,
const Eigen::VectorXf &,
PointCloud &, bool = true) const override
{
};
bool
- doSamplesVerifyModel (const std::set<int> &,
+ doSamplesVerifyModel (const std::set<index_t> &,
const Eigen::VectorXf &,
const double) const override
{
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood (const std::vector<int> &samples) const override;
+ isSampleGood (const Indices &samples) const override;
/** \brief Computes an "optimal" sample distance threshold based on the
* principal directions of the input cloud.
*/
inline void
computeSampleDistanceThreshold (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices)
+ const Indices &indices)
{
// Compute the principal directions via PCA
Eigen::Vector4f xyz_centroid;
*/
void
estimateRigidTransformationSVD (const pcl::PointCloud<PointT> &cloud_src,
- const std::vector<int> &indices_src,
+ const Indices &indices_src,
const pcl::PointCloud<PointT> &cloud_tgt,
- const std::vector<int> &indices_tgt,
+ const Indices &indices_tgt,
Eigen::VectorXf &transform) const;
/** \brief Compute mappings between original indices of the input_/target_ clouds. */
#pragma once
#include <pcl/sample_consensus/sac_model_registration.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: pcl::SampleConsensusModelRegistration<PointT> (cloud, indices, random)
, projection_matrix_ (Eigen::Matrix3f::Identity ())
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers);
+ Indices &inliers);
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood (const std::vector<int> &samples) const;
+ isSampleGood (const Indices &samples) const;
/** \brief Computes an "optimal" sample distance threshold based on the
* principal directions of the input cloud.
*/
inline void
computeSampleDistanceThreshold (const PointCloudConstPtr&,
- const std::vector<int>&)
+ const Indices&)
{
//// Compute the principal directions via PCA
//Eigen::Vector4f xyz_centroid;
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelSphere (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
{
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all distances from the cloud data to a given sphere model.
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
* \todo implement this.
*/
void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const override;
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const override;
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood(const std::vector<int> &samples) const override;
+ isSampleGood(const Indices &samples) const override;
private:
struct OptimizationFunctor : pcl::Functor<float>
* \param[in] indices the indices of data points to evaluate
* \param[in] estimator pointer to the estimator object
*/
- OptimizationFunctor (const pcl::SampleConsensusModelSphere<PointT> *model, const std::vector<int>& indices) :
+ OptimizationFunctor (const pcl::SampleConsensusModelSphere<PointT> *model, const Indices& indices) :
pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
/** Cost function to be minimized
}
const pcl::SampleConsensusModelSphere<PointT> *model_;
- const std::vector<int> &indices_;
+ const Indices &indices_;
};
};
}
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelStick (const PointCloudConstPtr &cloud,
- const std::vector<int> &indices,
+ const Indices &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random)
{
* \param[out] model_coefficients the resultant model coefficients
*/
bool
- computeModelCoefficients (const std::vector<int> &samples,
+ computeModelCoefficients (const Indices &samples,
Eigen::VectorXf &model_coefficients) const override;
/** \brief Compute all squared distances from the cloud data to a given stick model.
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
- std::vector<int> &inliers) override;
+ Indices &inliers) override;
/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[out] optimized_coefficients the resultant recomputed coefficients after optimization
*/
void
- optimizeModelCoefficients (const std::vector<int> &inliers,
+ optimizeModelCoefficients (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const override;
* \param[in] copy_data_fields set to true if we need to copy the other data fields
*/
void
- projectPoints (const std::vector<int> &inliers,
+ projectPoints (const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields = true) const override;
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
bool
- doSamplesVerifyModel (const std::set<int> &indices,
+ doSamplesVerifyModel (const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const override;
* \param[in] samples the resultant index samples
*/
bool
- isSampleGood (const std::vector<int> &samples) const override;
+ isSampleGood (const Indices &samples) const override;
};
}
using PointCloud = typename Search<PointT>::PointCloud;
using PointCloudConstPtr = typename Search<PointT>::PointCloudConstPtr;
- using IndicesPtr = shared_ptr<std::vector<int> >;
- using IndicesConstPtr = shared_ptr<const std::vector<int> >;
+ using IndicesPtr = pcl::IndicesPtr;
+ using IndicesConstPtr = pcl::IndicesConstPtr;
using pcl::search::Search<PointT>::input_;
using pcl::search::Search<PointT>::indices_;
struct Entry
{
- Entry (int idx, float dist) : index (idx), distance (dist) {}
+ Entry (index_t idx, float dist) : index (idx), distance (dist) {}
Entry () : index (0), distance (0) {}
- unsigned index;
+ index_t index;
float distance;
inline bool
* \return number of neighbors found
*/
int
- nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const override;
+ nearestKSearch (const PointT &point, int k, Indices &k_indices, std::vector<float> &k_distances) const override;
/** \brief Search for all the nearest neighbors of the query point in a given radius.
* \param[in] point the given query point
*/
int
radiusSearch (const PointT& point, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const override;
private:
int
- denseKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const;
+ denseKSearch (const PointT &point, int k, Indices &k_indices, std::vector<float> &k_distances) const;
int
- sparseKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const;
+ sparseKSearch (const PointT &point, int k, Indices &k_indices, std::vector<float> &k_distances) const;
int
denseRadiusSearch (const PointT& point, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const;
int
sparseRadiusSearch (const PointT& point, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const;
};
}
* search.setInputCloud (target);
*
* // Do search
- * std::vector<std::vector<int> > k_indices;
+ * std::vector<Indices> k_indices;
* std::vector<std::vector<float> > k_sqr_distances;
- * search.nearestKSearch (*query, std::vector<int> (), 2, k_indices, k_sqr_distances);
+ * search.nearestKSearch (*query, Indices (), 2, k_indices, k_sqr_distances);
* \endcode
*
* \author Andreas Muetzel
using PointCloud = typename Search<PointT>::PointCloud;
using PointCloudConstPtr = typename Search<PointT>::PointCloudConstPtr;
- using typename Search<PointT>::IndicesPtr;
- using typename Search<PointT>::IndicesConstPtr;
-
using MatrixPtr = shared_ptr<flann::Matrix<float> >;
using MatrixConstPtr = shared_ptr<const flann::Matrix<float> >;
void
setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ()) override;
+ using Search<PointT>::nearestKSearch;
+
/** \brief Search for the k-nearest neighbors for the given query point.
* \param[in] point the given query point
* \param[in] k the number of neighbors to search for
* \return number of neighbors found
*/
int
- nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const override;
+ nearestKSearch (const PointT &point, int k, Indices &k_indices, std::vector<float> &k_sqr_distances) const override;
/** \brief Search for the k-nearest neighbors for the given query point.
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
*/
void
- nearestKSearch (const PointCloud& cloud, const std::vector<int>& indices, int k,
- std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances) const override;
+ nearestKSearch (const PointCloud& cloud, const Indices& indices, int k,
+ std::vector<Indices>& k_indices, std::vector< std::vector<float> >& k_sqr_distances) const override;
/** \brief Search for all the nearest neighbors of the query point in a given radius.
* \param[in] point the given query point
*/
int
radiusSearch (const PointT& point, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const override;
/** \brief Search for the k-nearest neighbors for the given query point.
* \param[in] max_nn if given, bounds the maximum returned neighbors to this value
*/
void
- radiusSearch (const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
+ radiusSearch (const PointCloud& cloud, const Indices& indices, double radius, std::vector<Indices>& k_indices,
std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn=0) const override;
/** \brief Provide a pointer to the point representation to use to convert points into k-D vectors.
int dim_;
- std::vector<int> index_mapping_;
+ Indices index_mapping_;
bool identity_mapping_;
};
*
*/
-#ifndef PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_
-#define PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_
+#pragma once
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/search/brute_force.h>
#include <queue>
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::BruteForce<PointT>::nearestKSearch (
- const PointT& point, int k, std::vector<int>& k_indices, std::vector<float>& k_distances) const
+ const PointT& point, int k, Indices& k_indices, std::vector<float>& k_distances) const
{
assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::BruteForce<PointT>::denseKSearch (
- const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const
+ const PointT &point, int k, Indices &k_indices, std::vector<float> &k_distances) const
{
// container for first k elements -> O(1) for insertion, since order not required here
std::vector<Entry> result;
std::priority_queue<Entry> queue;
if (indices_)
{
- std::vector<int>::const_iterator iIt =indices_->begin ();
- std::vector<int>::const_iterator iEnd = indices_->begin () + std::min (static_cast<unsigned> (k), static_cast<unsigned> (indices_->size ()));
+ auto iIt = indices_->cbegin ();
+ auto iEnd = indices_->cbegin () + std::min (static_cast<unsigned> (k), static_cast<unsigned> (indices_->size ()));
for (; iIt != iEnd; ++iIt)
result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point)));
// add the rest
Entry entry;
- for (; iIt != indices_->end (); ++iIt)
+ for (; iIt != indices_->cend (); ++iIt)
{
entry.distance = getDistSqr (input_->points[*iIt], point);
if (queue.top ().distance > entry.distance)
else
{
Entry entry;
- for (entry.index = 0; entry.index < std::min (static_cast<unsigned> (k), static_cast<unsigned> (input_->size ())); ++entry.index)
+ for (entry.index = 0; entry.index < std::min<pcl::index_t> (k, input_->size ()); ++entry.index)
{
entry.distance = getDistSqr (input_->points[entry.index], point);
result.push_back (entry);
queue = std::priority_queue<Entry> (result.begin (), result.end ());
// add the rest
- for (; entry.index < input_->size (); ++entry.index)
+ for (; entry.index < static_cast<pcl::index_t>(input_->size ()); ++entry.index)
{
entry.distance = getDistSqr (input_->points[entry.index], point);
if (queue.top ().distance > entry.distance)
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::BruteForce<PointT>::sparseKSearch (
- const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const
+ const PointT &point, int k, Indices &k_indices, std::vector<float> &k_distances) const
{
// result used to collect the first k neighbors -> unordered
std::vector<Entry> result;
std::priority_queue<Entry> queue;
if (indices_)
{
- std::vector<int>::const_iterator iIt =indices_->begin ();
- for (; iIt != indices_->end () && result.size () < static_cast<unsigned> (k); ++iIt)
+ auto iIt =indices_->cbegin ();
+ for (; iIt != indices_->cend () && result.size () < static_cast<unsigned> (k); ++iIt)
{
if (std::isfinite (input_->points[*iIt].x))
result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point)));
// either we have k elements, or there are none left to iterate >in either case we're fine
// add the rest
Entry entry;
- for (; iIt != indices_->end (); ++iIt)
+ for (; iIt != indices_->cend (); ++iIt)
{
if (!std::isfinite (input_->points[*iIt].x))
continue;
else
{
Entry entry;
- for (entry.index = 0; entry.index < input_->size () && result.size () < static_cast<unsigned> (k); ++entry.index)
+ for (entry.index = 0; (entry.index < static_cast<pcl::index_t>(input_->size ())) && (result.size () < static_cast<std::size_t> (k)); ++entry.index)
{
if (std::isfinite (input_->points[entry.index].x))
{
queue = std::priority_queue<Entry> (result.begin (), result.end ());
// add the rest
- for (; entry.index < input_->size (); ++entry.index)
+ for (; entry.index < static_cast<pcl::index_t>(input_->size ()); ++entry.index)
{
if (!std::isfinite (input_->points[entry.index].x))
continue;
template <typename PointT> int
pcl::search::BruteForce<PointT>::denseRadiusSearch (
const PointT& point, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
radius *= radius;
float distance;
if (indices_)
{
- for (std::vector<int>::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt)
+ for (const auto& idx : *indices_)
{
- distance = getDistSqr (input_->points[*iIt], point);
+ distance = getDistSqr (input_->points[idx], point);
if (distance <= radius)
{
- k_indices.push_back (*iIt);
+ k_indices.push_back (idx);
k_sqr_distances.push_back (distance);
if (k_indices.size () == max_nn) // max_nn = 0 -> never true
break;
template <typename PointT> int
pcl::search::BruteForce<PointT>::sparseRadiusSearch (
const PointT& point, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
radius *= radius;
float distance;
if (indices_)
{
- for (std::vector<int>::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt)
+ for (const auto& idx : *indices_)
{
- if (!std::isfinite (input_->points[*iIt].x))
+ if (!std::isfinite (input_->points[idx].x))
continue;
- distance = getDistSqr (input_->points[*iIt], point);
+ distance = getDistSqr (input_->points[idx], point);
if (distance <= radius)
{
- k_indices.push_back (*iIt);
+ k_indices.push_back (idx);
k_sqr_distances.push_back (distance);
if (k_indices.size () == max_nn) // never true if max_nn = 0
break;
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::BruteForce<PointT>::radiusSearch (
- const PointT& point, double radius, std::vector<int> &k_indices,
+ const PointT& point, double radius, Indices &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn) const
{
assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
}
#define PCL_INSTANTIATE_BruteForce(T) template class PCL_EXPORTS pcl::search::BruteForce<T>;
-
-#endif //PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename FlannDistance> int
-pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, std::vector<int> &indices, std::vector<float> &dists) const
+pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, Indices &indices, std::vector<float> &dists) const
{
assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally
bool can_cast = point_representation_->isTrivial ();
indices.resize (k,-1);
if (dists.size() != static_cast<unsigned int> (k))
dists.resize (k);
- flann::Matrix<int> i (&indices[0],1,k);
+ flann::Matrix<index_t> i (&indices[0],1,k);
flann::Matrix<float> d (&dists[0],1,k);
int result = index_->knnSearch (m,i,d,k, p);
{
for (std::size_t i = 0; i < static_cast<unsigned int> (k); ++i)
{
- int& neighbor_index = indices[i];
+ auto& neighbor_index = indices[i];
neighbor_index = index_mapping_[neighbor_index];
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename FlannDistance> void
pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (
- const PointCloud& cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> >& k_indices,
+ const PointCloud& cloud, const Indices& indices, int k, std::vector<Indices>& k_indices,
std::vector< std::vector<float> >& k_sqr_distances) const
{
if (indices.empty ())
{
for (auto &k_index : k_indices)
{
- for (int &neighbor_index : k_index)
+ for (auto &neighbor_index : k_index)
{
neighbor_index = index_mapping_[neighbor_index];
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename FlannDistance> int
pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (const PointT& point, double radius,
- std::vector<int> &indices, std::vector<float> &distances,
+ Indices &indices, std::vector<float> &distances,
unsigned int max_nn) const
{
assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
p.eps = eps_;
p.max_neighbors = max_nn > 0 ? max_nn : -1;
p.checks = checks_;
- std::vector<std::vector<int> > i (1);
+ std::vector<Indices> i (1);
std::vector<std::vector<float> > d (1);
int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
if (!identity_mapping_)
{
- for (int &neighbor_index : indices)
+ for (auto &neighbor_index : indices)
{
neighbor_index = index_mapping_ [neighbor_index];
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename FlannDistance> void
pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (
- const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
+ const PointCloud& cloud, const Indices& indices, double radius, std::vector<Indices>& k_indices,
std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
{
if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
{
for (auto &k_index : k_indices)
{
- for (int &neighbor_index : k_index)
+ for (auto &neighbor_index : k_index)
{
neighbor_index = index_mapping_[neighbor_index];
}
continue;
}
- index_mapping_.push_back (static_cast<int> (i)); // If the returned index should be for the indices vector
+ index_mapping_.push_back (static_cast<index_t> (i)); // If the returned index should be for the indices vector
point_representation_->vectorize (point, cloud_ptr);
cloud_ptr += dim_;
float* cloud_ptr = input_flann_->ptr();
for (std::size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
{
- int cloud_index = (*indices_)[indices_index];
+ index_t cloud_index = (*indices_)[indices_index];
const PointT& point = (*input_)[cloud_index];
// Check if the point is invalid
if (!point_representation_->isValid (point))
continue;
}
- index_mapping_.push_back (static_cast<int> (indices_index)); // If the returned index should be for the indices vector
+ index_mapping_.push_back (static_cast<index_t> (indices_index)); // If the returned index should be for the indices vector
point_representation_->vectorize (point, cloud_ptr);
cloud_ptr += dim_;
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, class Tree> int
pcl::search::KdTree<PointT,Tree>::nearestKSearch (
- const PointT &point, int k, std::vector<int> &k_indices,
+ const PointT &point, int k, Indices &k_indices,
std::vector<float> &k_sqr_distances) const
{
return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances));
template <typename PointT, class Tree> int
pcl::search::KdTree<PointT,Tree>::radiusSearch (
const PointT& point, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
return (tree_->radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn));
*
*/
-#ifndef PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
-#define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
+#pragma once
#include <pcl/search/organized.h>
#include <pcl/common/eigen.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/common/time.h>
#include <Eigen/Eigenvalues>
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const PointT &query,
const double radius,
- std::vector<int> &k_indices,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
int k,
- std::vector<int> &k_indices,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances) const
{
assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
// if upper line of the rectangle is visible and x-extend is not 0
if (yBegin >= 0 && yBegin < static_cast<int> (input_->height))
{
- int idx = yBegin * input_->width + xFrom;
- int idxTo = idx + xTo - xFrom;
+ index_t idx = yBegin * input_->width + xFrom;
+ index_t idxTo = idx + xTo - xFrom;
for (; idx < idxTo; ++idx)
stop = testPoint (query, k, results, idx) || stop;
}
// if lower line of the rectangle is visible
if (yEnd > 0 && yEnd <= static_cast<int> (input_->height))
{
- int idx = (yEnd - 1) * input_->width + xFrom;
- int idxTo = idx + xTo - xFrom;
+ index_t idx = (yEnd - 1) * input_->width + xFrom;
+ index_t idxTo = idx + xTo - xFrom;
for (; idx < idxTo; ++idx)
stop = testPoint (query, k, results, idx) || stop;
{
if (xBegin >= 0 && xBegin < static_cast<int> (input_->width))
{
- int idx = yFrom * input_->width + xBegin;
- int idxTo = yTo * input_->width + xBegin;
+ index_t idx = yFrom * input_->width + xBegin;
+ index_t idxTo = yTo * input_->width + xBegin;
for (; idx < idxTo; idx += input_->width)
stop = testPoint (query, k, results, idx) || stop;
if (xEnd > 0 && xEnd <= static_cast<int> (input_->width))
{
- int idx = yFrom * input_->width + xEnd - 1;
- int idxTo = yTo * input_->width + xEnd - 1;
+ index_t idx = yFrom * input_->width + xEnd - 1;
+ index_t idxTo = yTo * input_->width + xEnd - 1;
for (; idx < idxTo; idx += input_->width)
stop = testPoint (query, k, results, idx) || stop;
const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, unsigned (1));
const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, unsigned (1));
- std::vector<int> indices;
+ Indices indices;
indices.reserve (input_->size () >> (pyramid_level_ << 1));
for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip)
if (std::abs (residual_sqr) > eps_ * float (indices.size ()))
{
- PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ());
+ PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ());
return;
}
}
#define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;
-#endif
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::Search<PointT>::nearestKSearch (
- const PointCloud &cloud, int index, int k,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
+ const PointCloud &cloud, index_t index, int k,
+ Indices &k_indices, std::vector<float> &k_sqr_distances) const
{
- assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!");
+ assert (index >= 0 && index < static_cast<index_t> (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!");
return (nearestKSearch (cloud.points[index], k, k_indices, k_sqr_distances));
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::Search<PointT>::nearestKSearch (
- int index, int k,
- std::vector<int> &k_indices,
+ index_t index, int k,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances) const
{
if (!indices_)
{
- assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!");
+ assert (index >= 0 && index < static_cast<index_t> (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!");
return (nearestKSearch (input_->points[index], k, k_indices, k_sqr_distances));
}
- assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!");
- if (index >= static_cast<int> (indices_->size ()) || index < 0)
+ assert (index >= 0 && index < static_cast<index_t> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!");
+ if (index >= static_cast<index_t> (indices_->size ()) || index < 0)
return (0);
return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_sqr_distances));
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::search::Search<PointT>::nearestKSearch (
- const PointCloud& cloud, const std::vector<int>& indices,
- int k, std::vector< std::vector<int> >& k_indices,
+ const PointCloud& cloud, const Indices& indices,
+ int k, std::vector<Indices>& k_indices,
std::vector< std::vector<float> >& k_sqr_distances) const
{
if (indices.empty ())
k_indices.resize (cloud.size ());
k_sqr_distances.resize (cloud.size ());
for (std::size_t i = 0; i < cloud.size (); i++)
- nearestKSearch (cloud, static_cast<int> (i), k, k_indices[i], k_sqr_distances[i]);
+ nearestKSearch (cloud, static_cast<index_t> (i), k, k_indices[i], k_sqr_distances[i]);
}
else
{
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::Search<PointT>::radiusSearch (
- const PointCloud &cloud, int index, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ const PointCloud &cloud, index_t index, double radius,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
- assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!");
+ assert (index >= 0 && index < static_cast<index_t> (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!");
return (radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::Search<PointT>::radiusSearch (
- int index, double radius, std::vector<int> &k_indices,
+ index_t index, double radius, Indices &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn ) const
{
if (!indices_)
{
- assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in radiusSearch!");
+ assert (index >= 0 && index < static_cast<index_t> (input_->points.size ()) && "Out-of-bounds error in radiusSearch!");
return (radiusSearch (input_->points[index], radius, k_indices, k_sqr_distances, max_nn));
}
- assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
+ assert (index >= 0 && index < static_cast<index_t> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn));
}
template <typename PointT> void
pcl::search::Search<PointT>::radiusSearch (
const PointCloud& cloud,
- const std::vector<int>& indices,
+ const Indices& indices,
double radius,
- std::vector< std::vector<int> >& k_indices,
+ std::vector<Indices>& k_indices,
std::vector< std::vector<float> > &k_sqr_distances,
unsigned int max_nn) const
{
k_indices.resize (cloud.size ());
k_sqr_distances.resize (cloud.size ());
for (std::size_t i = 0; i < cloud.size (); i++)
- radiusSearch (cloud, static_cast<int> (i), radius,k_indices[i], k_sqr_distances[i], max_nn);
+ radiusSearch (cloud, static_cast<index_t> (i), radius,k_indices[i], k_sqr_distances[i], max_nn);
}
else
{
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::search::Search<PointT>::sortResults (
- std::vector<int>& indices, std::vector<float>& distances) const
+ Indices& indices, std::vector<float>& distances) const
{
- std::vector<int> order (indices.size ());
+ Indices order (indices.size ());
for (std::size_t idx = 0; idx < order.size (); ++idx)
- order [idx] = static_cast<int> (idx);
+ order [idx] = static_cast<index_t> (idx);
Compare compare (distances);
sort (order.begin (), order.end (), compare);
- std::vector<int> sorted (indices.size ());
+ Indices sorted (indices.size ());
for (std::size_t idx = 0; idx < order.size (); ++idx)
sorted [idx] = indices[order [idx]];
using PointCloud = typename Search<PointT>::PointCloud;
using PointCloudConstPtr = typename Search<PointT>::PointCloudConstPtr;
- using typename Search<PointT>::IndicesPtr;
- using typename Search<PointT>::IndicesConstPtr;
-
using pcl::search::Search<PointT>::indices_;
using pcl::search::Search<PointT>::input_;
using pcl::search::Search<PointT>::getIndices;
*/
int
nearestKSearch (const PointT &point, int k,
- std::vector<int> &k_indices,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances) const override;
/** \brief Search for all the nearest neighbors of the query point in a given radius.
*/
int
radiusSearch (const PointT& point, double radius,
- std::vector<int> &k_indices,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const override;
protected:
using Ptr = shared_ptr<pcl::search::Octree<PointT,LeafTWrap,BranchTWrap,OctreeT> >;
using ConstPtr = shared_ptr<const pcl::search::Octree<PointT,LeafTWrap,BranchTWrap,OctreeT> >;
- using typename Search<PointT>::IndicesPtr;
- using typename Search<PointT>::IndicesConstPtr;
-
using PointCloud = pcl::PointCloud<PointT>;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
* \return number of neighbors found
*/
inline int
- nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
+ nearestKSearch (const PointCloud &cloud, index_t index, int k, Indices &k_indices,
std::vector<float> &k_sqr_distances) const override
{
return (tree_->nearestKSearch (cloud, index, k, k_indices, k_sqr_distances));
* \return number of neighbors found
*/
inline int
- nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
+ nearestKSearch (const PointT &point, int k, Indices &k_indices,
std::vector<float> &k_sqr_distances) const override
{
return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances));
* \return number of neighbors found
*/
inline int
- nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const override
+ nearestKSearch (index_t index, int k, Indices &k_indices, std::vector<float> &k_sqr_distances) const override
{
return (tree_->nearestKSearch (index, k, k_indices, k_sqr_distances));
}
*/
inline int
radiusSearch (const PointCloud &cloud,
- int index,
+ index_t index,
double radius,
- std::vector<int> &k_indices,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const override
{
inline int
radiusSearch (const PointT &p_q,
double radius,
- std::vector<int> &k_indices,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const override
{
* \return number of neighbors found in radius
*/
inline int
- radiusSearch (int index, double radius, std::vector<int> &k_indices,
+ radiusSearch (index_t index, double radius, Indices &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const override
{
tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, max_nn);
* \return number of neighbors found
*/
inline void
- approxNearestSearch (const PointCloudConstPtr &cloud, int query_index, int &result_index,
+ approxNearestSearch (const PointCloudConstPtr &cloud, index_t query_index, index_t &result_index,
float &sqr_distance)
{
return (tree_->approxNearestSearch (cloud->points[query_index], result_index, sqr_distance));
* \param[out] sqr_distance the resultant squared distance to the neighboring point
*/
inline void
- approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance)
+ approxNearestSearch (const PointT &p_q, index_t &result_index, float &sqr_distance)
{
return (tree_->approxNearestSearch (p_q, result_index, sqr_distance));
}
* \return number of neighbors found
*/
inline void
- approxNearestSearch (int query_index, int &result_index, float &sqr_distance)
+ approxNearestSearch (index_t query_index, index_t &result_index, float &sqr_distance)
{
return (tree_->approxNearestSearch (query_index, result_index, sqr_distance));
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
- using typename Search<PointT>::IndicesConstPtr;
using Ptr = shared_ptr<pcl::search::OrganizedNeighbor<PointT> >;
using ConstPtr = shared_ptr<const pcl::search::OrganizedNeighbor<PointT> >;
if (indices_ && !indices_->empty())
{
mask_.assign (input_->size (), 0);
- for (std::vector<int>::const_iterator iIt = indices_->begin (); iIt != indices_->end (); ++iIt)
- mask_[*iIt] = 1;
+ for (const auto& idx : *indices_)
+ mask_[idx] = 1;
}
else
mask_.assign (input_->size (), 1);
int
radiusSearch (const PointT &p_q,
double radius,
- std::vector<int> &k_indices,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const override;
int
nearestKSearch (const PointT &p_q,
int k,
- std::vector<int> &k_indices,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances) const override;
/** \brief projects a point into the image
struct Entry
{
- Entry (int idx, float dist) : index (idx), distance (dist) {}
+ Entry (index_t idx, float dist) : index (idx), distance (dist) {}
Entry () : index (0), distance (0) {}
- unsigned index;
+ index_t index;
float distance;
inline bool
* \return whether the top element changed or not.
*/
inline bool
- testPoint (const PointT& query, unsigned k, std::priority_queue<Entry>& queue, unsigned index) const
+ testPoint (const PointT& query, unsigned k, std::priority_queue<Entry>& queue, index_t index) const
{
const PointT& point = input_->points [index];
if (mask_ [index] && std::isfinite (point.x))
#pragma once
+#include <pcl/pcl_base.h> // for IndicesConstPtr
#include <pcl/point_cloud.h>
#include <pcl/for_each_type.h>
#include <pcl/common/concatenate.h>
using Ptr = shared_ptr<pcl::search::Search<PointT> >;
using ConstPtr = shared_ptr<const pcl::search::Search<PointT> >;
- using IndicesPtr = shared_ptr<std::vector<int> >;
- using IndicesConstPtr = shared_ptr<const std::vector<int> >;
+ using IndicesPtr = pcl::IndicesPtr;
+ using IndicesConstPtr = pcl::IndicesConstPtr;
/** Constructor. */
Search (const std::string& name = "", bool sorted = false);
* \return number of neighbors found
*/
virtual int
- nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
+ nearestKSearch (const PointT &point, int k, Indices &k_indices,
std::vector<float> &k_sqr_distances) const = 0;
/** \brief Search for k-nearest neighbors for the given query point.
*/
template <typename PointTDiff> inline int
nearestKSearchT (const PointTDiff &point, int k,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
+ Indices &k_indices, std::vector<float> &k_sqr_distances) const
{
PointT p;
copyPoint (point, p);
* \exception asserts in debug mode if the index is not between 0 and the maximum number of points
*/
virtual int
- nearestKSearch (const PointCloud &cloud, int index, int k,
- std::vector<int> &k_indices,
+ nearestKSearch (const PointCloud &cloud, index_t index, int k,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances) const;
/** \brief Search for k-nearest neighbors for the given query point (zero-copy).
* \exception asserts in debug mode if the index is not between 0 and the maximum number of points
*/
virtual int
- nearestKSearch (int index, int k,
- std::vector<int> &k_indices,
+ nearestKSearch (index_t index, int k,
+ Indices &k_indices,
std::vector<float> &k_sqr_distances) const;
/** \brief Search for the k-nearest neighbors for the given query point.
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
*/
virtual void
- nearestKSearch (const PointCloud& cloud, const std::vector<int>& indices,
- int k, std::vector< std::vector<int> >& k_indices,
+ nearestKSearch (const PointCloud& cloud, const Indices& indices,
+ int k, std::vector<Indices>& k_indices,
std::vector< std::vector<float> >& k_sqr_distances) const;
/** \brief Search for the k-nearest neighbors for the given query point. Use this method if the query points are of a different type than the points in the data set (e.g. PointXYZRGBA instead of PointXYZ).
* \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search.
*/
template <typename PointTDiff> void
- nearestKSearchT (const pcl::PointCloud<PointTDiff> &cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> > &k_indices,
+ nearestKSearchT (const pcl::PointCloud<PointTDiff> &cloud, const Indices& indices, int k, std::vector<Indices> &k_indices,
std::vector< std::vector<float> > &k_sqr_distances) const
{
// Copy all the data fields from the input cloud to the output one
pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (
cloud[i], pc[i]));
}
- nearestKSearch (pc,std::vector<int>(),k,k_indices,k_sqr_distances);
+ nearestKSearch (pc,Indices(),k,k_indices,k_sqr_distances);
}
else
{
pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (
cloud[indices[i]], pc[i]));
}
- nearestKSearch (pc,std::vector<int>(),k,k_indices,k_sqr_distances);
+ nearestKSearch (pc,Indices(),k,k_indices,k_sqr_distances);
}
}
* \return number of neighbors found in radius
*/
virtual int
- radiusSearch (const PointT& point, double radius, std::vector<int>& k_indices,
+ radiusSearch (const PointT& point, double radius, Indices& k_indices,
std::vector<float>& k_sqr_distances, unsigned int max_nn = 0) const = 0;
/** \brief Search for all the nearest neighbors of the query point in a given radius.
* \return number of neighbors found in radius
*/
template <typename PointTDiff> inline int
- radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices,
+ radiusSearchT (const PointTDiff &point, double radius, Indices &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
{
PointT p;
* \exception asserts in debug mode if the index is not between 0 and the maximum number of points
*/
virtual int
- radiusSearch (const PointCloud &cloud, int index, double radius,
- std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
+ radiusSearch (const PointCloud &cloud, index_t index, double radius,
+ Indices &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0) const;
/** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy).
* \exception asserts in debug mode if the index is not between 0 and the maximum number of points
*/
virtual int
- radiusSearch (int index, double radius, std::vector<int> &k_indices,
+ radiusSearch (index_t index, double radius, Indices &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
/** \brief Search for all the nearest neighbors of the query point in a given radius.
*/
virtual void
radiusSearch (const PointCloud& cloud,
- const std::vector<int>& indices,
+ const Indices& indices,
double radius,
- std::vector< std::vector<int> >& k_indices,
+ std::vector<Indices>& k_indices,
std::vector< std::vector<float> > &k_sqr_distances,
unsigned int max_nn = 0) const;
*/
template <typename PointTDiff> void
radiusSearchT (const pcl::PointCloud<PointTDiff> &cloud,
- const std::vector<int>& indices,
+ const Indices& indices,
double radius,
- std::vector< std::vector<int> > &k_indices,
+ std::vector<Indices> &k_indices,
std::vector< std::vector<float> > &k_sqr_distances,
unsigned int max_nn = 0) const
{
pc.resize (cloud.size ());
for (std::size_t i = 0; i < cloud.size (); ++i)
pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (cloud[i], pc[i]));
- radiusSearch (pc, std::vector<int> (), radius, k_indices, k_sqr_distances, max_nn);
+ radiusSearch (pc, Indices (), radius, k_indices, k_sqr_distances, max_nn);
}
else
{
pc.resize (indices.size ());
for (std::size_t i = 0; i < indices.size (); ++i)
pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (cloud[indices[i]], pc[i]));
- radiusSearch (pc, std::vector<int>(), radius, k_indices, k_sqr_distances, max_nn);
+ radiusSearch (pc, Indices(), radius, k_indices, k_sqr_distances, max_nn);
}
}
protected:
void
- sortResults (std::vector<int>& indices, std::vector<float>& distances) const;
+ sortResults (Indices& indices, std::vector<float>& distances) const;
PointCloudConstPtr input_;
IndicesConstPtr indices_;
}
bool
- operator () (int first, int second) const
+ operator () (index_t first, index_t second) const
{
return (distances_ [first] < distances_[second]);
}
#ifndef Q_MOC_RUN
// Marking all Boost headers as system headers to remove warnings
#include <boost/version.hpp>
-#include <boost/make_shared.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/multi_array.hpp>
#include <boost/ptr_container/ptr_list.hpp>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/search/pcl_search.h>
/** \brief Provide a pointer to the search object.
* \param[in] tree a pointer to the spatial search object.
*/
- inline void
- setSearchMethod (const SearcherPtr &tree)
- {
- searcher_ = tree;
+ inline void
+ setSearchMethod (const SearcherPtr &tree)
+ {
+ searcher_ = tree;
}
- /** \brief Get a pointer to the search method used.
+ /** \brief Get a pointer to the search method used.
*/
- inline const SearcherPtr&
- getSearchMethod () const
- {
- return searcher_;
+ inline const SearcherPtr&
+ getSearchMethod () const
+ {
+ return searcher_;
}
/** \brief Set the condition that needs to hold for neighboring points to be considered part of the same cluster.
* \param[in] condition_function The condition function that needs to hold for clustering
*/
inline void
- setConditionFunction (bool (*condition_function) (const PointT&, const PointT&, float))
+ setConditionFunction (bool (*condition_function) (const PointT&, const PointT&, float))
{
condition_function_ = condition_function;
}
#define PCL_INSTANTIATE_CPCSegmentation(T) template class PCL_EXPORTS pcl::CPCSegmentation<T>;
namespace pcl
-{
+{
/** \brief A segmentation algorithm partitioning a supervoxel graph. It uses planar cuts induced by local concavities for the recursive segmentation. Cuts are estimated using locally constrained directed RANSAC.
* \note If you use this in a scientific work please cite the following paper:
* M. Schoeler, J. Papon, F. Woergoetter
public:
CPCSegmentation ();
-
+
~CPCSegmentation ();
/** \brief Merge supervoxels using cuts through local convexities. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method.
- * \note There are three ways to retrieve the segmentation afterwards (inherited from \ref LCCPSegmentation): \ref relabelCloud, \ref getSegmentSupervoxelMap and \ref getSupervoxelSegmentMap*/
+ * \note There are three ways to retrieve the segmentation afterwards (inherited from \ref LCCPSegmentation): \ref relabelCloud, \ref getSupervoxelToSegmentMap and \ref getSupervoxelToSegmentMap */
void
segment ();
/** \brief Set the number of iterations for the weighted RANSAC step (best cut estimations)
* \param[in] ransac_iterations The number of iterations */
- inline void
+ inline void
setRANSACIterations (const std::uint32_t ransac_iterations)
{
ransac_itrs_ = ransac_iterations;
}
- private:
+ private:
/** \brief Used in for CPC to find and fit cutting planes to the pointcloud.
* \note Is used recursively
/** \brief Use clean cutting */
bool use_clean_cutting_;
-
+
/** \brief Iterations for RANSAC */
std::uint32_t ransac_itrs_;
-
-
-/******************************************* Directional weighted RANSAC declarations ******************************************************************/
+
+
+/******************************************* Directional weighted RANSAC declarations ******************************************************************/
/** \brief @b WeightedRandomSampleConsensus represents an implementation of the Directionally Weighted RANSAC algorithm, as described in: "Constrained Planar Cuts - Part Segmentation for Point Clouds", CVPR 2015, M. Schoeler, J. Papon, F. Wörgötter.
* \note It only uses points with a weight > 0 for the model calculation, but uses all points for the evaluation (scoring of the model)
* Only use in conjunction with sac_model_plane
* \author Markus Schoeler (mschoeler@web.de)
* \ingroup segmentation
*/
-
+
class WeightedRandomSampleConsensus : public SampleConsensus<WeightSACPointType>
{
using SampleConsensusModelPtr = SampleConsensusModel<WeightSACPointType>::Ptr;
* \param[in] model a Sample Consensus model
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
- WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model,
+ WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model,
bool random = false)
: SampleConsensus<WeightSACPointType> (model, random)
{
full_cloud_pt_indices_.reset (new std::vector<int> (* (sac_model_->getIndices ())));
point_cloud_ptr_ = sac_model_->getInputCloud ();
}
-
+
/** \brief weight each positive weight point by the inner product between the normal and the plane normal */
bool use_directed_weights_;
-
+
/** \brief vector of weights assigned to points. Set by the setWeights-method */
std::vector<double> weights_;
-
+
/** \brief The indices used for estimating the RANSAC model. Only those whose weight is > 0 */
pcl::IndicesPtr model_pt_indices_;
-
+
/** \brief The complete list of indices used for the model evaluation */
pcl::IndicesPtr full_cloud_pt_indices_;
-
+
/** \brief Pointer to the input PointCloud */
pcl::PointCloud<WeightSACPointType>::ConstPtr point_cloud_ptr_;
-
+
/** \brief Highest score found so far */
double best_score_;
};
-
+
};
}
#elif defined(PCL_ONLY_CORE_POINT_TYPES)
//pcl::PointXYZINormal is not a core point type (so we cannot use the precompiled classes here)
#include <pcl/sample_consensus/impl/sac_model_plane.hpp>
- #include <pcl/segmentation/impl/extract_clusters.hpp>
+ #include <pcl/segmentation/impl/extract_clusters.hpp>
#endif // PCL_NO_PRECOMPILE / PCL_ONLY_CORE_POINT_TYPES
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
namespace pcl
{
- /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients,
+ /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients,
* for use in planar segmentation.
* In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
*
public:
using PointCloud = typename Comparator<PointT>::PointCloud;
using PointCloudConstPtr = typename Comparator<PointT>::PointCloudConstPtr;
-
+
using PointCloudN = pcl::PointCloud<PointNT>;
using PointCloudNPtr = typename PointCloudN::Ptr;
using PointCloudNConstPtr = typename PointCloudN::ConstPtr;
-
+
using Ptr = shared_ptr<EdgeAwarePlaneComparator<PointT, PointNT> >;
using ConstPtr = shared_ptr<const EdgeAwarePlaneComparator<PointT, PointNT> >;
{
}
- /** \brief Empty constructor for PlaneCoefficientComparator.
+ /** \brief Empty constructor for PlaneCoefficientComparator.
* \param[in] distance_map the distance map to use
*/
- EdgeAwarePlaneComparator (const float *distance_map) :
+ EdgeAwarePlaneComparator (const float *distance_map) :
distance_map_ (distance_map),
distance_map_threshold_ (5),
curvature_threshold_ (0.04f),
}
/** \brief Destructor for PlaneCoefficientComparator. */
-
+
~EdgeAwarePlaneComparator ()
{
}
- /** \brief Set a distance map to use. For an example of a valid distance map see
- * \ref OrganizedIntegralImageNormalEstimation
+ /** \brief Set a distance map to use. For an example of a valid distance map see
+ * IntegralImageNormalEstimation::getDistanceMap
* \param[in] distance_map the distance map to use
*/
inline void
{
return (euclidean_distance_threshold_);
}
-
+
protected:
/** \brief Compare two neighboring points, by using normal information, curvature, and euclidean distance information.
* \param[in] idx1 The index of the first point.
dist_threshold *= z * z;
euclidean_dist_threshold *= z * z;
}
-
+
float dx = input_->points[idx1].x - input_->points[idx2].x;
float dy = input_->points[idx1].y - input_->points[idx2].y;
float dz = input_->points[idx1].z - input_->points[idx2].z;
bool curvature_ok = normals_->points[idx1].curvature < curvature_threshold_;
bool plane_d_ok = std::abs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < dist_threshold;
-
- if (distance_map_[idx1] < distance_map_threshold_)
+
+ if (distance_map_[idx1] < distance_map_threshold_)
curvature_ok = false;
-
+
return (dist_ok && normal_ok && curvature_ok && plane_d_ok);
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
#include <pcl/segmentation/boost.h>
#include <pcl/segmentation/comparator.h>
-#include <pcl/point_types.h>
+
namespace pcl
{
using experimental::EuclideanClusterComparator<PointT, PointLT>::setExcludeLabels;
/** \brief Default constructor for EuclideanClusterComparator. */
- PCL_DEPRECATED("remove PointNT from template parameters")
+ PCL_DEPRECATED(1, 12, "remove PointNT from template parameters")
EuclideanClusterComparator ()
: normals_ ()
, angular_threshold_ (0.0f)
/** \brief Provide a pointer to the input normals.
* \param[in] normals the input normal cloud
*/
- PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
+ PCL_DEPRECATED(1, 12, "EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
inline void
setInputNormals (const PointCloudNConstPtr& normals) { normals_ = normals; }
/** \brief Get the input normals. */
- PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
+ PCL_DEPRECATED(1, 12, "EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
inline PointCloudNConstPtr
getInputNormals () const { return (normals_); }
/** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
* \param[in] angular_threshold the tolerance in radians
*/
- PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
+ PCL_DEPRECATED(1, 12, "EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
inline void
setAngularThreshold (float angular_threshold)
{
}
/** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
- PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
+ PCL_DEPRECATED(1, 12, "EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
inline float
getAngularThreshold () const { return (std::acos (angular_threshold_) ); }
/** \brief Set labels in the label cloud to exclude.
* \param[in] exclude_labels a vector of bools corresponding to whether or not a given label should be considered
*/
- PCL_DEPRECATED("use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead")
+ PCL_DEPRECATED(1, 12, "use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead")
void
setExcludeLabels (const std::vector<bool>& exclude_labels)
{
- exclude_labels_ = boost::make_shared<std::set<std::uint32_t> > ();
+ exclude_labels_ = pcl::make_shared<std::set<std::uint32_t> > ();
for (std::size_t i = 0; i < exclude_labels.size (); ++i)
if (exclude_labels[i])
exclude_labels_->insert (i);
#pragma once
+#include <pcl/memory.h>
#include <pcl/point_cloud.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#pragma once
-#include <pcl/common/angles.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
+#include <pcl/common/angles.h>
#include <pcl/segmentation/comparator.h>
-#include <boost/make_shared.hpp>
+
namespace pcl
{
void
setPlaneCoeffD (std::vector<float>& plane_coeff_d)
{
- plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d);
+ plane_coeff_d_ = pcl::make_shared<std::vector<float> >(plane_coeff_d);
}
/** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */
std::vector<float> window_sizes;
std::vector<int> half_sizes;
int iteration = 0;
- int half_size = 0.0f;
- float window_size = 0.0f;
- float height_threshold = 0.0f;
+ float window_size = 0.0f;
while (window_size < max_window_size_)
{
// Determine the initial window size.
- if (exponential_)
- half_size = static_cast<int> (std::pow (static_cast<float> (base_), iteration));
- else
- half_size = (iteration+1) * base_;
+ int half_size = (exponential_) ? (static_cast<int> (std::pow (static_cast<float> (base_), iteration))) : ((iteration+1) * base_);
window_size = 2 * half_size + 1;
// Calculate the height threshold to be used in the next iteration.
- if (iteration == 0)
- height_threshold = initial_distance_;
- else
- height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
+ float height_threshold = (iteration == 0) ? (initial_distance_) : (slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_);
// Enforce max distance on height threshold
if (height_threshold > max_distance_)
Eigen::MatrixXf Zf (rows, cols);
Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(A, global_min) \
+ num_threads(threads_)
for (int i = 0; i < (int)input_->points.size (); ++i)
{
// ...then test for lower points within the cell
pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
// Apply the morphological opening operation at the current window size.
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(A, cols, half_sizes, i, rows, Z) \
+ num_threads(threads_)
for (int row = 0; row < rows; ++row)
{
int rs, re;
}
}
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(cols, half_sizes, i, rows, Z, Zf) \
+ num_threads(threads_)
for (int row = 0; row < rows; ++row)
{
int rs, re;
// ----------------------------------//
// -------- -------------------//
- pcl::PointCloud<pcl::PointXYZRGBL> tmp_cloud;
- tmp_cloud = *filtered_anno_;
-
// create dense CRF
DenseCrf crf (N, n_labels);
/*
+ pcl::PointCloud<pcl::PointXYZRGBL> tmp_cloud;
+ tmp_cloud = *filtered_anno_;
+
bool c = true;
for (std::size_t i = 0; i < tmp_cloud.points.size (); i++)
{
*
*/
-#ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP
-#define PCL_SEGMENTATION_IMPL_GRABCUT_HPP
+#pragma once
+#include <pcl/common/distances.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/search/organized.h>
#include <pcl/search/kdtree.h>
-#include <pcl/common/distances.h>
+
namespace pcl
{
- template <>
- float squaredEuclideanDistance (const pcl::segmentation::grabcut::Color &c1,
- const pcl::segmentation::grabcut::Color &c2)
- {
- return ((c1.r-c2.r)*(c1.r-c2.r)+(c1.g-c2.g)*(c1.g-c2.g)+(c1.b-c2.b)*(c1.b-c2.b));
- }
+
+template <>
+float squaredEuclideanDistance (const pcl::segmentation::grabcut::Color &c1,
+ const pcl::segmentation::grabcut::Color &c2)
+{
+ return ((c1.r-c2.r)*(c1.r-c2.r)+(c1.g-c2.g)*(c1.g-c2.g)+(c1.b-c2.b)*(c1.b-c2.b));
}
+namespace segmentation
+{
+
template <typename PointT>
-pcl::segmentation::grabcut::Color::Color (const PointT& p)
+grabcut::Color::Color (const PointT& p)
{
r = static_cast<float> (p.r) / 255.0;
g = static_cast<float> (p.g) / 255.0;
}
template <typename PointT>
-pcl::segmentation::grabcut::Color::operator PointT () const
+grabcut::Color::operator PointT () const
{
PointT p;
p.r = static_cast<std::uint32_t> (r * 255);
return (p);
}
+} // namespace segmentation
+
template <typename PointT> void
-
pcl::GrabCut<PointT>::setInputCloud (const PointCloudConstPtr &cloud)
+GrabCut<PointT>::setInputCloud (const PointCloudConstPtr &cloud)
{
input_ = cloud;
}
template <typename PointT> bool
-
pcl::GrabCut<PointT>::initCompute ()
+GrabCut<PointT>::initCompute ()
{
using namespace pcl::segmentation::grabcut;
if (!pcl::PCLBase<PointT>::initCompute ())
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity)
+GrabCut<PointT>::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity)
{
graph_.addEdge (v1, v2, capacity, rev_capacity);
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity)
+GrabCut<PointT>::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity)
{
graph_.addSourceEdge (v, source_capacity);
graph_.addTargetEdge (v, sink_capacity);
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::setBackgroundPointsIndices (const PointIndicesConstPtr &indices)
+GrabCut<PointT>::setBackgroundPointsIndices (const PointIndicesConstPtr &indices)
{
using namespace pcl::segmentation::grabcut;
if (!initCompute ())
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::fitGMMs ()
+GrabCut<PointT>::fitGMMs ()
{
// Step 3: Build GMMs using Orchard-Bouman clustering algorithm
buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
}
template <typename PointT> int
-
pcl::GrabCut<PointT>::refineOnce ()
+GrabCut<PointT>::refineOnce ()
{
// Steps 4 and 5: Learn new GMMs from current segmentation
learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::refine ()
+GrabCut<PointT>::refine ()
{
std::size_t changed = indices_->size ();
}
template <typename PointT> int
-
pcl::GrabCut<PointT>::updateHardSegmentation ()
+GrabCut<PointT>::updateHardSegmentation ()
{
using namespace pcl::segmentation::grabcut;
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
+GrabCut<PointT>::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
{
using namespace pcl::segmentation::grabcut;
for (const int &index : indices->indices)
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::initGraph ()
+GrabCut<PointT>::initGraph ()
{
using namespace pcl::segmentation::grabcut;
const int number_of_indices = static_cast<int> (indices_->size ());
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::computeNLinksNonOrganized ()
+GrabCut<PointT>::computeNLinksNonOrganized ()
{
const int number_of_indices = static_cast<int> (indices_->size ());
for (int i_point = 0; i_point < number_of_indices; ++i_point)
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::computeNLinksOrganized ()
+GrabCut<PointT>::computeNLinksOrganized ()
{
for( unsigned int y = 0; y < image_->height; ++y )
{
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::computeBetaNonOrganized ()
+GrabCut<PointT>::computeBetaNonOrganized ()
{
float result = 0;
std::size_t edges = 0;
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::computeBetaOrganized ()
+GrabCut<PointT>::computeBetaOrganized ()
{
float result = 0;
std::size_t edges = 0;
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::computeL ()
+GrabCut<PointT>::computeL ()
{
L_ = 8*lambda_ + 1;
}
template <typename PointT> void
-
pcl::GrabCut<PointT>::extract (std::vector<pcl::PointIndices>& clusters)
+GrabCut<PointT>::extract (std::vector<pcl::PointIndices>& clusters)
{
using namespace pcl::segmentation::grabcut;
clusters.clear ();
clusters[0].indices.push_back (i);
}
-#endif
+} // namespace pcl
+
{
seg_label_to_neighbor_set_map_.clear ();
- //The vertices in the supervoxel adjacency list are the supervoxel centroids
- std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
- vertex_iterator_range = boost::vertices (sv_adjacency_list_);
-
std::uint32_t current_segLabel;
std::uint32_t neigh_segLabel;
+ VertexIterator sv_itr, sv_itr_end;
+ //The vertices in the supervoxel adjacency list are the supervoxel centroids
// For every Supervoxel..
- for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels
+ for(std::tie(sv_itr, sv_itr_end) = boost::vertices(sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
{
const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
current_segLabel = sv_label_to_seg_label_map_[sv_label];
+ AdjacencyIterator itr_neighbor, itr_neighbor_end;
// ..look at all neighbors and insert their labels into the neighbor set
- std::pair<AdjacencyIterator, AdjacencyIterator> neighbors = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_);
- for (AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor)
+ for (std::tie(itr_neighbor, itr_neighbor_end) = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_); itr_neighbor != itr_neighbor_end; ++itr_neighbor)
{
const std::uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor];
neigh_segLabel = sv_label_to_seg_label_map_[neigh_label];
std::set<std::uint32_t> filteredSegLabels;
- std::uint32_t largest_neigh_size = 0;
- std::uint32_t largest_neigh_seg_label = 0;
- std::uint32_t current_seg_label;
-
- std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
- vertex_iterator_range = boost::vertices (sv_adjacency_list_);
-
bool continue_filtering = true;
while (continue_filtering)
continue_filtering = false;
unsigned int nr_filtered = 0;
+ VertexIterator sv_itr, sv_itr_end;
// Iterate through all supervoxels, check if they are in a "small" segment -> change label to largest neighborID
- for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels
+ for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
{
const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
- current_seg_label = sv_label_to_seg_label_map_[sv_label];
- largest_neigh_seg_label = current_seg_label;
- largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
+ std::uint32_t current_seg_label = sv_label_to_seg_label_map_[sv_label];
+ std::uint32_t largest_neigh_seg_label = current_seg_label;
+ std::uint32_t largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
if (nr_neighbors == 0)
sv_label_to_seg_label_map_[sv_label] = 0;
}
+ VertexIterator sv_itr, sv_itr_end;
// Perform depth search on the graph and recursively group all supervoxels with convex connections
//The vertices in the supervoxel adjacency list are the supervoxel centroids
- std::pair< VertexIterator, VertexIterator> vertex_iterator_range;
- vertex_iterator_range = boost::vertices (sv_adjacency_list_);
-
// Note: *sv_itr is of type " boost::graph_traits<VoxelAdjacencyList>::vertex_descriptor " which it nothing but a typedef of std::size_t..
unsigned int segment_label = 1; // This starts at 1, because 0 is reserved for errors
- for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels
+ for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
{
const VertexID sv_vertex_id = *sv_itr;
const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
sv_label_to_seg_label_map_[sv_label] = segment_label;
seg_label_to_sv_list_map_[segment_label].insert (sv_label);
+ OutEdgeIterator out_Edge_itr, out_Edge_itr_end;
// Iterate through all neighbors of this supervoxel and check whether they should be merged with the current supervoxel
- std::pair<OutEdgeIterator, OutEdgeIterator> out_edge_iterator_range;
- out_edge_iterator_range = boost::out_edges (query_point_id, sv_adjacency_list_); // adjacent vertices to node (*itr) in graph sv_adjacency_list_
- for (OutEdgeIterator out_Edge_itr = out_edge_iterator_range.first; out_Edge_itr != out_edge_iterator_range.second; ++out_Edge_itr)
+ // boost::out_edges (query_point_id, sv_adjacency_list_): adjacent vertices to node (*itr) in graph sv_adjacency_list_
+ for (std::tie(out_Edge_itr, out_Edge_itr_end) = boost::out_edges (query_point_id, sv_adjacency_list_); out_Edge_itr != out_Edge_itr_end; ++out_Edge_itr)
{
const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
if (k_arg == 0)
return;
- unsigned int kcount = 0;
-
EdgeIterator edge_itr, edge_itr_end, next_edge;
- boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
-
- std::pair<OutEdgeIterator, OutEdgeIterator> source_neighbors_range;
- std::pair<OutEdgeIterator, OutEdgeIterator> target_neighbors_range;
-
// Check all edges in the graph for k-convexity
- for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
+ for (std::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_), next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
{
- next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
+ ++next_edge; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
bool is_convex = sv_adjacency_list_[*edge_itr].is_convex;
if (is_convex) // If edge is (0-)convex
{
- kcount = 0;
+ unsigned int kcount = 0;
const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
- source_neighbors_range = boost::out_edges (source, sv_adjacency_list_);
- target_neighbors_range = boost::out_edges (target, sv_adjacency_list_);
-
+ OutEdgeIterator source_neighbors_itr, source_neighbors_itr_end;
// Find common neighbors, check their connection
- for (OutEdgeIterator source_neighbors_itr = source_neighbors_range.first; source_neighbors_itr != source_neighbors_range.second; ++source_neighbors_itr) // For all supervoxels
+ for (std::tie(source_neighbors_itr, source_neighbors_itr_end) = boost::out_edges (source, sv_adjacency_list_); source_neighbors_itr != source_neighbors_itr_end; ++source_neighbors_itr) // For all supervoxels
{
VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
- for (OutEdgeIterator target_neighbors_itr = target_neighbors_range.first; target_neighbors_itr != target_neighbors_range.second; ++target_neighbors_itr) // For all supervoxels
+ OutEdgeIterator target_neighbors_itr, target_neighbors_itr_end;
+ for (std::tie(target_neighbors_itr, target_neighbors_itr_end) = boost::out_edges (target, sv_adjacency_list_); target_neighbors_itr != target_neighbors_itr_end; ++target_neighbors_itr) // For all supervoxels
{
VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
if (source_neighbor_ID == target_neighbor_ID) // Common neighbor
{
EdgeIterator edge_itr, edge_itr_end, next_edge;
- boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg);
-
- for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
+ for (std::tie(edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg), next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
{
- next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
+ ++next_edge; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
}
clusters_.clear ();
- bool success = true;
if ( !graph_is_valid_ )
{
- success = buildGraph ();
+ bool success = buildGraph ();
if (!success)
{
deinitCompute ();
if ( !unary_potentials_are_valid_ )
{
- success = recalculateUnaryPotentials ();
+ bool success = recalculateUnaryPotentials ();
if (!success)
{
deinitCompute ();
if ( !binary_potentials_are_valid_ )
{
- success = recalculateBinaryPotentials ();
+ bool success = recalculateBinaryPotentials ();
if (!success)
{
deinitCompute ();
for (int i_point = 0; i_point < number_of_indices; i_point++)
{
- int point_index = (*indices_)[i_point];
+ index_t point_index = (*indices_)[i_point];
double source_weight = 0.0;
double sink_weight = 0.0;
calculateUnaryPotential (point_index, source_weight, sink_weight);
search_->setInputCloud (input_, indices_);
for (int i_point = 0; i_point < number_of_indices; i_point++)
{
- int point_index = (*indices_)[i_point];
+ index_t point_index = (*indices_)[i_point];
search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances);
for (std::size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++)
{
{
double min_dist_to_foreground = std::numeric_limits<double>::max ();
//double min_dist_to_background = std::numeric_limits<double>::max ();
- double closest_foreground_point[2];
- closest_foreground_point[0] = closest_foreground_point[1] = 0;
//double closest_background_point[] = {0.0, 0.0};
double initial_point[] = {0.0, 0.0};
if (min_dist_to_foreground > dist)
{
min_dist_to_foreground = dist;
- closest_foreground_point[0] = foreground_points_[i_point].x;
- closest_foreground_point[1] = foreground_points_[i_point].y;
}
}
colored_cloud->is_dense = input_->is_dense;
pcl::PointXYZRGB point;
- int point_index = 0;
for (int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++)
{
- point_index = clusters_[0].indices[i_point];
+ index_t point_index = clusters_[0].indices[i_point];
point.x = *(input_->points[point_index].data);
point.y = *(input_->points[point_index].data + 1);
point.z = *(input_->points[point_index].data + 2);
for (int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++)
{
- point_index = clusters_[1].indices[i_point];
+ index_t point_index = clusters_[1].indices[i_point];
point.x = *(input_->points[point_index].data);
point.y = *(input_->points[point_index].data + 1);
point.z = *(input_->points[point_index].data + 2);
if (!initCompute ())
return;
+ // Check that the normals are present
+ if (!normals_)
+ {
+ PCL_ERROR( "[pcl::%s::segment] Must specify normals.\n", getClassName().c_str());
+ return;
+ }
+
// Check that we got the same number of points and normals
if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
{
*
*/
-#ifndef PCL_SEGMENTATION_REGION_GROWING_HPP_
-#define PCL_SEGMENTATION_REGION_GROWING_HPP_
+#pragma once
#include <pcl/segmentation/region_growing.h>
-#include <pcl/search/search.h>
-#include <pcl/search/kdtree.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+#include <pcl/search/search.h>
+#include <pcl/search/kdtree.h>
#include <queue>
#include <list>
#define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;
-#endif // PCL_SEGMENTATION_REGION_GROWING_HPP_
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_stick.h>
+#include <pcl/memory.h> // for static_pointer_cast
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SACSegmentation<PointT>::segment (PointIndices &inliers, ModelCoefficients &model_coefficients)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_, random_));
- typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle =
boost::static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_);
+ typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_);
double min_radius, max_radius;
model_circle->getRadiusLimits (min_radius, max_radius);
if (radius_min_ != min_radius && radius_max_ != max_radius)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCircle3D<PointT> (input_, *indices_));
- typename SampleConsensusModelCircle3D<PointT>::Ptr model_circle3d =
boost::static_pointer_cast<SampleConsensusModelCircle3D<PointT> > (model_);
+ typename SampleConsensusModelCircle3D<PointT>::Ptr model_circle3d = static_pointer_cast<SampleConsensusModelCircle3D<PointT> > (model_);
double min_radius, max_radius;
model_circle3d->getRadiusLimits (min_radius, max_radius);
if (radius_min_ != min_radius && radius_max_ != max_radius)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_, random_));
- typename SampleConsensusModelSphere<PointT>::Ptr model_sphere =
boost::static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_);
+ typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_);
double min_radius, max_radius;
model_sphere->getRadiusLimits (min_radius, max_radius);
if (radius_min_ != min_radius && radius_max_ != max_radius)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_, random_));
- typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel =
boost::static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_);
+ typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_);
if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_, random_));
- typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular =
boost::static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_);
+ typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_);
if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_, random_));
- typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel =
boost::static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_);
+ typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_);
if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_, random_));
- typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder =
boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_);
+ typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_);
// Set the input normals
model_cylinder->setInputNormals (normals_);
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_, random_));
- typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals =
boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_);
+ typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_);
// Set the input normals
model_normals->setInputNormals (normals_);
if (distance_weight_ != model_normals->getNormalDistanceWeight ())
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_, random_));
- typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals =
boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_);
+ typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_);
// Set the input normals
model_normals->setInputNormals (normals_);
if (distance_weight_ != model_normals->getNormalDistanceWeight ())
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCone<PointT, PointNT> (input_, *indices_, random_));
- typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone =
boost::static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_);
+ typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone = static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_);
// Set the input normals
model_cone->setInputNormals (normals_);
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelNormalSphere<PointT, PointNT> (input_, *indices_, random_));
- typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere =
boost::static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_);
+ typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere = static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_);
// Set the input normals
model_normals_sphere->setInputNormals (normals_);
double min_radius, max_radius;
*
*/
-#ifndef PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
-#define PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
+#pragma once
#include <pcl/segmentation/segment_differences.h>
+
#include <pcl/common/io.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
#define PCL_INSTANTIATE_SegmentDifferences(T) template class PCL_EXPORTS pcl::SegmentDifferences<T>;
#define PCL_INSTANTIATE_getPointCloudDifference(T) template PCL_EXPORTS void pcl::getPointCloudDifference<T>(const pcl::PointCloud<T> &, double, const typename pcl::search::Search<T>::Ptr &, pcl::PointCloud<T> &);
-#endif // PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_
{
/** \brief Describes the difference of normals of the two supervoxels being connected*/
float normal_difference;
-
+
/** \brief Describes if a connection is convex or concave*/
bool is_convex;
-
+
/** \brief Describes if a connection is valid for the segment growing. Usually convex connections are and concave connection are not. Due to k-concavity a convex connection can be invalidated*/
bool is_valid;
-
+
/** \brief Additional member used for the CPC algorithm. If edge has already induced a cut, it should be ignored for further cutting.*/
bool used_for_cutting;
-
+
EdgeProperties () :
- normal_difference (0), is_convex (false), is_valid (false), used_for_cutting (false)
+ normal_difference (0), is_convex (false), is_valid (false), used_for_cutting (false)
{
}
};
void
reset ();
-
+
/** \brief Set the supervoxel clusters as well as the adjacency graph for the segmentation.Those parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref segment method.
* \param[in] supervoxel_clusters_arg Map of < supervoxel labels, supervoxels >
- * \param[in] label_adjacency_arg The graph defining the supervoxel adjacency relations
+ * \param[in] label_adjacency_arg The graph defining the supervoxel adjacency relations
* \note Implicitly calls \ref reset */
inline void
setInputSupervoxels (const std::map<std::uint32_t, typename pcl::Supervoxel<PointT>::Ptr> &supervoxel_clusters_arg,
prepareSegmentation (supervoxel_clusters_arg, label_adjacency_arg); // after this, sv_adjacency_list_ can be used to access adjacency list
supervoxels_set_ = true;
}
-
+
/** \brief Merge supervoxels using local convexity. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method.
- * \note There are three ways to retrieve the segmentation afterwards: \ref relabelCloud, \ref getSegmentSupervoxelMap and \ref getSupervoxelSegmentMap*/
+ * \note There are three ways to retrieve the segmentation afterwards: \ref relabelCloud, \ref getSegmentToSupervoxelMap and \ref getSupervoxelToSegmentMap. */
void
segment ();
- /** \brief Relabels cloud with supervoxel labels with the computed segment labels. labeled_cloud_arg should be created using the \ref getLabeledCloud method of the \ref SupervoxelClustering class.
+ /** \brief Relabels cloud with supervoxel labels with the computed segment labels. labeled_cloud_arg should be created using SupervoxelClustering::getLabeledCloud.
* \param[in,out] labeled_cloud_arg Cloud to relabel */
void
relabelCloud (pcl::PointCloud<pcl::PointXYZL> &labeled_cloud_arg);
-
+
/** \brief Get map<SegmentID, std::set<SuperVoxel IDs> >
* \param[out] segment_supervoxel_map_arg The output container. On error the map is empty. */
inline void
segment_supervoxel_map_arg = std::map<std::uint32_t, std::set<std::uint32_t> > ();
}
}
-
+
/** \brief Get map<Supervoxel_ID, Segment_ID>
* \param[out] supervoxel_segment_map_arg The output container. On error the map is empty. */
inline void
supervoxel_segment_map_arg = std::map<std::uint32_t, std::uint32_t> ();
}
}
-
+
/** \brief Get map <SegmentID, std::set<Neighboring SegmentIDs> >
* \param[out] segment_adjacency_map_arg map < SegmentID, std::set< Neighboring SegmentIDs> >. On error the map is empty. */
inline void
segment_adjacency_map_arg = std::map<std::uint32_t, std::set<std::uint32_t> > ();
}
}
-
+
/** \brief Get normal threshold
* \return The concavity tolerance angle in [deg] that is currently set */
inline float
{
return (concavity_tolerance_threshold_);
}
-
+
/** \brief Get the supervoxel adjacency graph with classified edges (boost::adjacency_list).
* \param[out] adjacency_list_arg The supervoxel adjacency list with classified (convex/concave) edges. On error the list is empty. */
inline void
adjacency_list_arg = pcl::LCCPSegmentation<PointT>::SupervoxelAdjacencyList ();
}
}
-
+
/** \brief Set normal threshold
* \param[in] concavity_tolerance_threshold_arg the concavity tolerance angle in [deg] to set */
inline void
{
k_factor_ = k_factor_arg;
}
-
+
/** \brief Set the value \ref min_segment_size_ used in \ref mergeSmallSegments
* \param[in] min_segment_size_arg Segments smaller than this size will be merged */
inline void
* \note The vertices in the supervoxel adjacency list are the supervoxel centroids */
void
doGrouping ();
-
+
/** \brief Assigns neighbors of the query point to the same group as the query point. Recursive part of \ref doGrouping. Grouping is done by a depth-search of nodes in the adjacency-graph.
* \param[in] queryPointID ID of point whose neighbors will be considered for grouping
* \param[in] group_label ID of the group/segment the queried point belongs to */
/** \brief Marks if valid grouping data (\ref sv_adjacency_list_, \ref sv_label_to_seg_label_map_, \ref processed_) is available */
bool grouping_data_valid_;
-
+
/** \brief Marks if supervoxels have been set by calling \ref setInputSupervoxels */
bool supervoxels_set_;
/** \brief Determines if we use the sanity check which tries to find and invalidate singular connected patches*/
bool use_sanity_check_;
-
+
/** \brief Seed resolution of the supervoxels (used only for smoothness check) */
float seed_resolution_;
/** \brief Factor used for k-convexity */
std::uint32_t k_factor_;
-
+
/** \brief Minimum segment size */
std::uint32_t min_segment_size_;
- /** \brief Stores which supervoxel labels were already visited during recursive grouping.
+ /** \brief Stores which supervoxel labels were already visited during recursive grouping.
* \note processed_[sv_Label] = false (default)/true (already processed) */
std::map<std::uint32_t, bool> processed_;
/** \brief map from the supervoxel labels to the supervoxel objects */
std::map<std::uint32_t, typename pcl::Supervoxel<PointT>::Ptr> sv_label_to_supervoxel_map_;
- /** \brief Storing relation between original SuperVoxel Labels and new segmantion labels.
+ /** \brief Storing relation between original SuperVoxel Labels and new segmantion labels.
* \note sv_label_to_seg_label_map_[old_labelID] = new_labelID */
std::map<std::uint32_t, std::uint32_t> sv_label_to_seg_label_map_;
- /** \brief map <Segment Label, std::set <SuperVoxel Labels> > */
+ /** \brief map Segment Label to a set of Supervoxel Labels */
std::map<std::uint32_t, std::set<std::uint32_t> > seg_label_to_sv_list_map_;
/** \brief map < SegmentID, std::set< Neighboring segment labels> > */
#pragma once
#include <pcl/segmentation/boost.h>
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
/** \brief Allows to set search method for finding KNN.
* The graph is build such way that it contains the edges that connect point and its KNN.
- * \param[in] search search method that will be used for finding KNN.
+ * \param[in] tree search method that will be used for finding KNN.
*/
void
setSearchMethod (const KdTreePtr& tree);
getNumberOfNeighbours () const;
/** \brief Allows to set the number of neighbours to find.
- * \param[in] number_of_neighbours new number of neighbours
+ * \param[in] neighbour_number new number of neighbours
*/
void
setNumberOfNeighbours (unsigned int neighbour_number);
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/common/angles.h>
+#include <pcl/common/utils.h>
#include <pcl/PointIndices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/plane_coefficient_comparator.h>
* \param [in] labels The labels produced by the initial segmentation
* \param [in] label_indices The list of indices corresponding to each label
*/
- PCL_DEPRECATED("centroids and covariances parameters are not used; they are deprecated and will be removed in future releases")
+ PCL_DEPRECATED(1, 12, "centroids and covariances parameters are not used; they are deprecated and will be removed in future releases")
void
refine (std::vector<ModelCoefficients>& model_coefficients,
std::vector<PointIndices>& inlier_indices,
- std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& /*centroids*/,
- std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& /*covariances*/,
+ std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
+ std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
PointCloudLPtr& labels,
std::vector<pcl::PointIndices>& label_indices)
{
+ pcl::utils::ignore(centroids);
+ pcl::utils::ignore(covariances);
refine(model_coefficients, inlier_indices, labels, label_indices);
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/segmentation/region_3d.h>
#include <pcl/geometry/planar_polygon.h>
#pragma once
#include <pcl/common/angles.h>
+#include <pcl/memory.h> // for pcl::make_shared, pcl::shared_ptr
#include <pcl/pcl_macros.h>
-#include <pcl/segmentation/boost.h>
#include <pcl/segmentation/comparator.h>
namespace pcl
public:
using PointCloud = typename Comparator<PointT>::PointCloud;
using PointCloudConstPtr = typename Comparator<PointT>::PointCloudConstPtr;
-
+
using PointCloudN = pcl::PointCloud<PointNT>;
using PointCloudNPtr = typename PointCloudN::Ptr;
using PointCloudNConstPtr = typename PointCloudN::ConstPtr;
-
+
using Ptr = shared_ptr<PlaneCoefficientComparator<PointT, PointNT> >;
using ConstPtr = shared_ptr<const PlaneCoefficientComparator<PointT, PointNT> >;
using pcl::Comparator<PointT>::input_;
-
+
/** \brief Empty constructor for PlaneCoefficientComparator. */
PlaneCoefficientComparator ()
: normals_ ()
/** \brief Constructor for PlaneCoefficientComparator.
* \param[in] plane_coeff_d a reference to a vector of d coefficients of plane equations. Must be the same size as the input cloud and input normals. a, b, and c coefficients are in the input normals.
*/
- PlaneCoefficientComparator (shared_ptr<std::vector<float> >& plane_coeff_d)
+ PlaneCoefficientComparator (shared_ptr<std::vector<float> >& plane_coeff_d)
: normals_ ()
, plane_coeff_d_ (plane_coeff_d)
, angular_threshold_ (pcl::deg2rad (2.0f))
, z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f) )
{
}
-
+
/** \brief Destructor for PlaneCoefficientComparator. */
-
+
~PlaneCoefficientComparator ()
{
}
- void
+ void
setInputCloud (const PointCloudConstPtr& cloud) override
{
input_ = cloud;
}
-
+
/** \brief Provide a pointer to the input normals.
* \param[in] normals the input normal cloud
*/
void
setPlaneCoeffD (std::vector<float>& plane_coeff_d)
{
- plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d);
+ plane_coeff_d_ = pcl::make_shared<std::vector<float> >(plane_coeff_d);
}
-
+
/** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */
const std::vector<float>&
getPlaneCoeffD () const
{
angular_threshold_ = std::cos (angular_threshold);
}
-
+
/** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
inline float
getAngularThreshold () const
* \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false)
*/
void
- setDistanceThreshold (float distance_threshold,
+ setDistanceThreshold (float distance_threshold,
bool depth_dependent = false)
{
distance_threshold_ = distance_threshold;
{
return (distance_threshold_);
}
-
+
/** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold,
* and the difference between the d component of the normals is less than distance threshold, else false
* \param idx1 The first index for the comparison
if (depth_dependent_)
{
Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
-
+
float z = vec.dot (z_axis_);
threshold *= z * z;
}
return ( (std::fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < threshold)
&& (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) );
}
-
+
protected:
PointCloudNConstPtr normals_;
shared_ptr<std::vector<float> > plane_coeff_d_;
#pragma once
-#include <pcl/segmentation/boost.h>
+#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/segmentation/plane_coefficient_comparator.h>
namespace pcl
{
- /** \brief PlaneRefinementComparator is a Comparator that operates on plane coefficients,
+ /** \brief PlaneRefinementComparator is a Comparator that operates on plane coefficients,
* for use in planar segmentation.
* In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
*
public:
using PointCloud = typename Comparator<PointT>::PointCloud;
using PointCloudConstPtr = typename Comparator<PointT>::PointCloudConstPtr;
-
+
using PointCloudN = pcl::PointCloud<PointNT>;
using PointCloudNPtr = typename PointCloudN::Ptr;
using PointCloudNConstPtr = typename PointCloudN::ConstPtr;
{
}
- /** \brief Empty constructor for PlaneCoefficientComparator.
+ /** \brief Empty constructor for PlaneCoefficientComparator.
* \param[in] models
* \param[in] refine_labels
*/
}
/** \brief Destructor for PlaneCoefficientComparator. */
-
~PlaneRefinementComparator ()
{
}
void
setModelCoefficients (std::vector<pcl::ModelCoefficients>& models)
{
- models_ =
boost::make_shared<std::vector<pcl::ModelCoefficients> >(models);
+ models_ =
pcl::make_shared<std::vector<pcl::ModelCoefficients> >(models);
}
/** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined.
{
refine_labels_ = refine_labels;
}
-
+
/** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined.
* \param[in] refine_labels A vector of bools 0-max_label, true if the label should be refined.
*/
void
setRefineLabels (std::vector<bool>& refine_labels)
{
- refine_labels_ = boost::make_shared<std::vector<bool> >(refine_labels);
+ refine_labels_ = pcl::make_shared<std::vector<bool> >(refine_labels);
}
/** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed.
{
label_to_model_ = label_to_model;
}
-
+
/** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed.
* \param[in] label_to_model A vector of size max_label, with the index of each corresponding model in models
*/
inline void
setLabelToModel (std::vector<int>& label_to_model)
{
- label_to_model_ = boost::make_shared<std::vector<int> >(label_to_model);
+ label_to_model_ = pcl::make_shared<std::vector<int> >(label_to_model);
}
/** \brief Get the vector of model coefficients to which we will compare. */
if (!((*refine_labels_)[current_label] && !(*refine_labels_)[next_label]))
return (false);
-
+
const pcl::ModelCoefficients& model_coeff = (*models_)[(*label_to_model_)[current_label]];
-
+
PointT pt = input_->points[idx2];
- double ptp_dist = std::fabs (model_coeff.values[0] * pt.x +
- model_coeff.values[1] * pt.y +
+ double ptp_dist = std::fabs (model_coeff.values[0] * pt.x +
+ model_coeff.values[1] * pt.y +
model_coeff.values[2] * pt.z +
model_coeff.values[3]);
-
+
// depth dependent
float threshold = distance_threshold_;
if (depth_dependent_)
{
//Eigen::Vector4f origin = input_->sensor_origin_;
Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();// - origin.head<3> ();
-
+
float z = vec.dot (z_axis_);
threshold *= z * z;
}
-
+
return (ptp_dist < threshold);
}
#include <Eigen/Core>
#include <vector>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/search/search.h>
/** \brief This destructor destroys the cloud, normals and search method used for
* finding KNN. In other words it frees memory.
*/
-
+
~RegionGrowing ();
/** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/segmentation/region_growing.h>
/** \brief Provide a pointer to the input dataset that contains the point normals of
* the XYZ dataset.
- * \param[in] normals the const boost shared pointer to a PointCloud message
+ * \param[in] normals the const shared pointer to a PointCloud message
*/
inline void
setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
pcl::PointCloud<PointT> &output);
template <typename PointT>
- PCL_DEPRECATED("tgt parameter is not used; it is deprecated and will be removed in future releases")
+ PCL_DEPRECATED(1, 12, "tgt parameter is not used; it is deprecated and will be removed in future releases")
inline void getPointCloudDifference (
const pcl::PointCloud<PointT> &src,
const pcl::PointCloud<PointT> & /* tgt */,
#include <boost/version.hpp>
#include <pcl/features/normal_3d.h>
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
Supervoxel () :
voxels_ (new pcl::PointCloud<PointT> ()),
normals_ (new pcl::PointCloud<Normal> ())
- { }
+ { }
using Ptr = shared_ptr<Supervoxel<PointT> >;
using ConstPtr = shared_ptr<const Supervoxel<PointT> >;
*/
SupervoxelClustering (float voxel_resolution, float seed_resolution);
- PCL_DEPRECATED("constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.")
+ PCL_DEPRECATED(1, 12, "constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.")
SupervoxelClustering (float voxel_resolution, float seed_resolution, bool) : SupervoxelClustering (voxel_resolution, seed_resolution) { }
/** \brief This destructor destroys the cloud, normals and search method used for
* color(it's random). Points that are unlabeled will be black
* \note This will expand the label_colors_ vector so that it can accommodate all labels
*/
- PCL_DEPRECATED("use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")
+ PCL_DEPRECATED(1, 12, "use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")
typename pcl::PointCloud<PointXYZRGBA>::Ptr
getColoredCloud () const
- {
+ {
return pcl::PointCloud<PointXYZRGBA>::Ptr (new pcl::PointCloud<PointXYZRGBA>);
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#pragma once
+#include <pcl/memory.h>
+#include <pcl/pcl_macros.h>
+
#include <Eigen/Dense>
#include <Eigen/StdVector>
-#include <boost/shared_ptr.hpp>
-
-#include <pcl/pcl_macros.h>
namespace pcl {
namespace simulation {
// Return the pose of the camera:
Eigen::Vector3d
- getYPR()
+ getYPR() const
{
return Eigen::Vector3d(yaw_, pitch_, roll_);
}
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_exports.h>
#include <Eigen/Core>
+
#include <GL/glew.h>
-#include <pcl/make_shared.h>
namespace pcl {
namespace simulation {
/** Add a new shader object to the program. */
bool
- addShaderText(const std::string& text, ShaderType shader_type);
+ addShaderText(const std::string& text, ShaderType shader_type) const;
/** Add a new shader object to the program. */
bool
/** Link the program. */
bool
- link();
+ link() const;
/** Return true if the program is linked. */
bool
/** Use the program. */
void
- use();
+ use() const;
// Set uniforms
void
setUniform(const std::string& name, bool v);
int
- getUniformLocation(const std::string& name);
+ getUniformLocation(const std::string& name) const;
void
printActiveUniforms();
printActiveAttribs();
GLuint
- programId()
+ programId() const
{
return program_id_;
}
#pragma once
+#include <pcl/io/pcd_io.h>
+#include <pcl/simulation/glsl_shader.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_config.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+
#if defined(_WIN32) && !defined(APIENTRY) && !defined(__CYGWIN__)
#define WIN32_LEAN_AND_MEAN 1
#include <windows.h>
#include <GL/glew.h>
-#include <pcl/pcl_config.h>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GL/glu.h>
#endif
-#include <boost/shared_ptr.hpp>
-#include <pcl/PolygonMesh.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/pcl_macros.h>
-#include <pcl/point_types.h>
-#include <pcl/simulation/glsl_shader.h>
-
namespace pcl {
namespace simulation {
/** Render the quad. */
void
- render();
+ render() const;
private:
GLuint quad_vbo_;
~TexturedQuad();
void
- setTexture(const std::uint8_t* data);
+ setTexture(const std::uint8_t* data) const;
void
render();
#pragma once
-#include <GL/glew.h>
-
+#include <pcl/common/transforms.h>
+#include <pcl/range_image/range_image_planar.h>
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/glsl_shader.h>
+#include <pcl/simulation/scene.h>
+#include <pcl/simulation/sum_reduce.h>
+#include <pcl/memory.h>
#include <pcl/pcl_config.h>
+#include <pcl/pcl_macros.h>
+
+#include <Eigen/StdVector>
+
+#include <GL/glew.h>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GL/glu.h>
#endif
-//#include <math.h>
-#include <Eigen/StdVector>
-
-#include <pcl/pcl_macros.h>
-//#include <pcl/win32_macros.h>
-#include <pcl/common/transforms.h>
-#include <pcl/range_image/range_image_planar.h>
-#include <pcl/simulation/camera.h>
-#include <pcl/simulation/glsl_shader.h>
-#include <pcl/simulation/scene.h>
-#include <pcl/simulation/sum_reduce.h>
-
namespace pcl {
namespace simulation {
+
class PCL_EXPORTS RangeLikelihood {
public:
using Ptr = shared_ptr<RangeLikelihood>;
applyCameraTransform(const Camera& camera);
void
- setupProjectionMatrix();
+ setupProjectionMatrix() const;
Scene::Ptr scene_;
int rows_;
#pragma once
-#include <boost/shared_ptr.hpp>
-
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
//#include <pcl/win32_macros.h>
-
#include <pcl/simulation/camera.h>
#include <pcl/simulation/model.h>
#pragma once
-#include <GL/glew.h>
-
+#include <pcl/simulation/glsl_shader.h>
+#include <pcl/simulation/model.h>
#include <pcl/pcl_config.h>
+
+#include <GL/glew.h>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#else
#include <GL/gl.h>
#endif
-#include <pcl/simulation/glsl_shader.h>
-#include <pcl/simulation/model.h>
-
namespace pcl {
namespace simulation {
-#include <iostream>
#include <pcl/simulation/camera.h>
+#include <iostream>
+
using namespace Eigen;
using namespace pcl::simulation;
* Author: hordurj
*/
+#include <pcl/simulation/glsl_shader.h>
+
#include <fstream>
#include <iostream>
-#include <pcl/simulation/glsl_shader.h>
using namespace pcl::simulation::gllib;
pcl::simulation::gllib::Program::~Program() {}
int
-pcl::simulation::gllib::Program::getUniformLocation(const std::string& name)
+pcl::simulation::gllib::Program::getUniformLocation(const std::string& name) const
{
return glGetUniformLocation(program_id_, name.c_str());
}
bool
pcl::simulation::gllib::Program::addShaderText(const std::string& text,
- ShaderType shader_type)
+ ShaderType shader_type) const
{
GLuint id;
GLint compile_status;
}
bool
-pcl::simulation::gllib::Program::link()
+pcl::simulation::gllib::Program::link() const
{
glLinkProgram(program_id_);
printProgramInfoLog(program_id_);
}
void
-pcl::simulation::gllib::Program::use()
+pcl::simulation::gllib::Program::use() const
{
glUseProgram(program_id_);
}
pcl::simulation::Quad::~Quad() { glDeleteBuffers(1, &quad_vbo_); }
void
-pcl::simulation::Quad::render()
+pcl::simulation::Quad::render() const
{
glBindBuffer(GL_ARRAY_BUFFER, quad_vbo_);
glEnableVertexAttribArray(0);
pcl::simulation::TexturedQuad::~TexturedQuad() { glDeleteTextures(1, &texture_); }
void
-pcl::simulation::TexturedQuad::setTexture(const std::uint8_t* data)
+pcl::simulation::TexturedQuad::setTexture(const std::uint8_t* data) const
{
glBindTexture(GL_TEXTURE_2D, texture_);
glTexImage2D(
-#include <ctime>
-#include <random>
+#include <pcl/common/time.h>
+#include <pcl/simulation/range_likelihood.h>
+#include <pcl/pcl_config.h>
#include <boost/math/distributions/normal.hpp>
#include <GL/glew.h>
-
-#include <pcl/pcl_config.h>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GL/glu.h>
#endif
-#include <pcl/common/time.h>
-#include <pcl/simulation/range_likelihood.h>
+#include <ctime>
+#include <random>
// For adding noise:
static std::minstd_rand rng(std::random_device{}());
}
void
-pcl::simulation::RangeLikelihood::setupProjectionMatrix()
+pcl::simulation::RangeLikelihood::setupProjectionMatrix() const
{
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
* pcl_sim_terminal_demo 2 ../../../../kmcl/models/table_models/meta_model.ply
*/
+#include <pcl/memory.h>
+
#include <Eigen/Dense>
-#include <boost/shared_ptr.hpp>
+
+#include "simulation_io.hpp"
+
#include <cmath>
#include <iostream>
#ifdef _WIN32
#include <windows.h>
#endif
-#include "simulation_io.hpp"
-
using namespace Eigen;
using namespace pcl;
using namespace pcl::console;
*
*/
+#include <pcl/common/common.h>
+#include <pcl/common/transforms.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/console/time.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/vtk_lib_io.h>
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/model.h>
+#include <pcl/simulation/range_likelihood.h>
+#include <pcl/simulation/scene.h>
+#include <pcl/surface/gp3.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_config.h>
+#include <pcl/point_types.h>
+
#include <Eigen/Dense>
-#include <boost/shared_ptr.hpp>
-#include <cmath>
-#include <iostream>
-#ifdef _WIN32
-#define WIN32_LEAN_AND_MEAN
-#include <windows.h>
-#endif
+
#include <GL/glew.h>
-#include <pcl/pcl_config.h>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GL/glut.h>
#endif
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-
-#include <pcl/common/common.h>
-#include <pcl/common/transforms.h>
-
-#include <pcl/features/normal_3d.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-#include <pcl/surface/gp3.h>
-
-// define the following in order to eliminate the deprecated headers warning
-#define VTK_EXCLUDE_STRSTREAM_HEADERS
-#include <pcl/io/vtk_lib_io.h>
-
-#include <pcl/simulation/camera.h>
-#include <pcl/simulation/model.h>
-#include <pcl/simulation/range_likelihood.h>
-#include <pcl/simulation/scene.h>
-
-#include <pcl/console/parse.h>
-#include <pcl/console/print.h>
-#include <pcl/console/time.h>
+#include <cmath>
+#include <iostream>
+#ifdef _WIN32
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
+#endif
using namespace Eigen;
using namespace pcl;
*
*/
+#include <pcl/common/common.h>
+#include <pcl/common/transforms.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/vtk_lib_io.h>
+#include <pcl/range_image/range_image_planar.h> // RangeImage
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/model.h>
+#include <pcl/simulation/range_likelihood.h>
+#include <pcl/simulation/scene.h>
+#include <pcl/surface/gp3.h>
+#include <pcl/visualization/cloud_viewer.h> // Pop-up viewer
+#include <pcl/memory.h>
+#include <pcl/pcl_config.h>
+#include <pcl/point_types.h>
+
#include <Eigen/Dense>
-#include <boost/shared_ptr.hpp>
-#include <cmath>
-#include <iostream>
-#include <thread>
-#ifdef _WIN32
-#define WIN32_LEAN_AND_MEAN
-#include <windows.h>
-#endif
+
#include <GL/glew.h>
-#include <pcl/pcl_config.h>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GL/glut.h>
#endif
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-
-#include "pcl/common/common.h"
-#include "pcl/common/transforms.h"
-
-#include <pcl/features/normal_3d.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-#include <pcl/surface/gp3.h>
-
-// define the following in order to eliminate the deprecated headers warning
-#define VTK_EXCLUDE_STRSTREAM_HEADERS
-#include <pcl/io/vtk_lib_io.h>
-
-#include "pcl/simulation/camera.h"
-#include "pcl/simulation/model.h"
-#include "pcl/simulation/range_likelihood.h"
-#include "pcl/simulation/scene.h"
-
-#include <pcl/console/parse.h>
-#include <pcl/console/print.h>
-
-// RangeImage:
-#include <pcl/range_image/range_image_planar.h>
-
-// Pop-up viewer
-#include <pcl/visualization/cloud_viewer.h>
+#include <cmath>
+#include <iostream>
+#include <thread>
+#ifdef _WIN32
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
+#endif
using namespace Eigen;
using namespace pcl;
* $Id: pcd_viewer.cpp 5094 2012-03-15 01:03:51Z rusu $
*
*/
-#include <Eigen/Dense>
-#include <Eigen/Geometry>
-#include <boost/shared_ptr.hpp>
-#include <cmath>
-#include <iostream>
-#ifdef _WIN32
-#define WIN32_LEAN_AND_MEAN
-#include <windows.h>
-#endif
-#include <GL/glew.h>
-
-#include <pcl/pcl_config.h>
-#ifdef OPENGL_IS_A_FRAMEWORK
-#include <OpenGL/gl.h>
-#else
-#include <GL/gl.h>
-#endif
-
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-
-#include "pcl/common/common.h"
-#include "pcl/common/transforms.h"
-
-#include <pcl/features/normal_3d.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-#include <pcl/surface/gp3.h>
-
-// define the following in order to eliminate the deprecated headers warning
-#define VTK_EXCLUDE_STRSTREAM_HEADERS
-#include <pcl/io/vtk_lib_io.h>
-
-#include "pcl/simulation/camera.h"
-#include "pcl/simulation/model.h"
-#include "pcl/simulation/range_likelihood.h"
-#include "pcl/simulation/scene.h"
-
-#include <pcl/console/parse.h>
-#include <pcl/console/print.h>
-#include <pcl/console/time.h>
-
-// RangeImage:
-#include <pcl/range_image/range_image_planar.h>
-
-// Pop-up viewer
-#include <pcl/visualization/cloud_viewer.h>
-
-#include <cfloat>
#include <pcl/common/common.h>
+#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/console/time.h>
+#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
+#include <pcl/io/vtk_lib_io.h>
+#include <pcl/range_image/range_image_planar.h> // RangeImage
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/model.h>
+#include <pcl/simulation/range_likelihood.h>
+#include <pcl/simulation/scene.h>
+#include <pcl/surface/gp3.h>
+#include <pcl/visualization/cloud_viewer.h> // Pop-up viewer
#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/visualization/keyboard_event.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl/visualization/point_picking_event.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_config.h>
+#include <pcl/point_types.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
#include <vtkPolyDataReader.h>
+#include <GL/glew.h>
+
+#ifdef OPENGL_IS_A_FRAMEWORK
+#include <OpenGL/gl.h>
+#else
+#include <GL/gl.h>
+#endif
+
+#include <cfloat>
+#include <cmath>
+#include <iostream>
+#ifdef _WIN32
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
+#endif
+
using namespace Eigen;
using namespace pcl;
using namespace pcl::console;
#include "simulation_io.hpp"
+
#include <pcl/io/png_io.h>
pcl::simulation::SimExample::SimExample(int argc, char** argv, int height, int width)
#pragma once
-#include <boost/shared_ptr.hpp>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/vtk_lib_io.h>
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/range_likelihood.h>
+#include <pcl/simulation/scene.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_config.h>
+#include <pcl/point_types.h>
#include <GL/glew.h>
-#include <pcl/pcl_config.h>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GL/glut.h>
#endif
-// define the following in order to eliminate the deprecated headers warning
-#define VTK_EXCLUDE_STRSTREAM_HEADERS
-#include <pcl/io/pcd_io.h>
-#include <pcl/io/vtk_lib_io.h>
-#include <pcl/point_types.h>
-
-#include <pcl/simulation/camera.h>
-#include <pcl/simulation/range_likelihood.h>
-#include <pcl/simulation/scene.h>
-
namespace pcl {
namespace simulation {
#pragma once
-#include <pcl/point_types.h>
#include <pcl/stereo/disparity_map_converter.h>
+#include <pcl/point_types.h>
namespace pcl {
#pragma once
-#include <cstring>
-#include <vector>
-
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
+#include <cstring>
+#include <vector>
+
namespace pcl {
/** \brief Compute point cloud from the disparity map.
#ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
#define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
+#include <pcl/common/intensity.h>
+#include <pcl/console/print.h>
#include <pcl/stereo/disparity_map_converter.h>
+#include <pcl/point_types.h>
#include <fstream>
#include <limits>
-#include <pcl/common/intensity.h>
-#include <pcl/console/print.h>
-#include <pcl/point_types.h>
-
template <typename PointT>
pcl::DisparityMapConverter<PointT>::DisparityMapConverter()
: center_x_(0.0f)
#pragma once
#include <pcl/common/time_trigger.h>
-#include <pcl/conversions.h>
#include <pcl/io/grabber.h>
-#include <pcl/point_cloud.h>
#include <pcl/stereo/stereo_matching.h>
+#include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
namespace pcl {
float frames_per_second,
bool repeat);
- /** \brief Copy constructor.
- * \param[in] src the Stereo Grabber base object to copy into this
- */
- StereoGrabberBase(const StereoGrabberBase& src) : impl_() { *this = src; }
-
- /** \brief Copy operator.
- * \param[in] src the Stereo Grabber base object to copy into this
- */
- StereoGrabberBase&
- operator=(const StereoGrabberBase& src)
- {
- impl_ = src.impl_;
- return (*this);
- }
-
/** \brief Virtual destructor. */
~StereoGrabberBase() noexcept;
#pragma once
-#include <algorithm>
-
#include <pcl/conversions.h>
#include <pcl/point_types.h>
+#include <algorithm>
+
namespace pcl {
template <class T>
short int
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include <pcl/stereo/digital_elevation_map.h>
-
#include <pcl/common/feature_histogram.h>
#include <pcl/console/print.h>
+#include <pcl/stereo/digital_elevation_map.h>
pcl::DigitalElevationMapBuilder::DigitalElevationMapBuilder()
: resolution_column_(64), resolution_disparity_(32), min_points_in_cell_(1)
*
*/
+#include <pcl/stereo/stereo_grabber.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
-#include <pcl/stereo/stereo_grabber.h>
///////////////////////////////////////////////////////////////////////////////////////////
//////////////////////// GrabberImplementation //////////////////////
)
set(POISSON_SOURCES
+ src/3rdparty/poisson4/bspline_data.cpp
src/3rdparty/poisson4/factor.cpp
src/3rdparty/poisson4/geometry.cpp
src/3rdparty/poisson4/marching_cubes_poisson.cpp
template <class T>
void ON_SimpleArray<T>::Insert( int i, const T& x )
{
- if( i >= 0 && i <= m_count )
+ if( i >= 0 && i <= m_count )
{
- if ( m_count == m_capacity )
+ if ( m_count == m_capacity )
{
int newcapacity = NewCapacity();
Reserve( newcapacity );
}
m_count++;
- Move( i+1, i, m_count-1-i );
+ Move( i+1, i, static_cast<unsigned int>(m_count)-1-i );
m_a[i] = x;
}
}
DestroyElement( m_a[m_count] );
m_count++;
if ( i < m_count-1 ) {
- Move( i+1, i, m_count-1-i );
+ Move( i+1, i, static_cast<unsigned int>(m_count)-1-i );
// This call to memset is ok even when T has a vtable
// because in-place construction is used later.
memset( (void*)(&m_a[i]), 0, sizeof(T) );
#include "ppolynomial.h"
+#include <cstdio>
+#include <cstring>
+
namespace pcl
{
namespace poisson
*/
#include "poisson_exceptions.h"
+#include "binary_node.h"
namespace pcl
{
{
if( functionCount )
{
- if( vvDotTable ) delete[] vvDotTable;
- if( dvDotTable ) delete[] dvDotTable;
- if( ddDotTable ) delete[] ddDotTable;
+ delete[] vvDotTable;
+ delete[] dvDotTable;
+ delete[] ddDotTable;
- if( valueTables ) delete[] valueTables;
- if( dValueTables ) delete[] dValueTables;
+ delete[] valueTables;
+ delete[] dValueTables;
- if( baseFunctions ) delete[] baseFunctions;
- if( baseBSplines ) delete[] baseBSplines;
+ delete[] baseFunctions;
+ delete[] baseBSplines;
}
vvDotTable = dvDotTable = ddDotTable = NULL;
valueTables = dValueTables=NULL;
template<int Degree,class Real>
void BSplineData<Degree,Real>::clearDotTables( int flags )
{
- if( (flags & VV_DOT_FLAG) && vvDotTable ) delete[] vvDotTable , vvDotTable = NULL;
- if( (flags & DV_DOT_FLAG) && dvDotTable ) delete[] dvDotTable , dvDotTable = NULL;
- if( (flags & DD_DOT_FLAG) && ddDotTable ) delete[] ddDotTable , ddDotTable = NULL;
+ if (flags & VV_DOT_FLAG) {
+ delete[] vvDotTable ; vvDotTable = NULL;
+ delete[] dvDotTable ; dvDotTable = NULL;
+ delete[] ddDotTable ; ddDotTable = NULL;
+ }
}
template< int Degree , class Real >
void BSplineData< Degree , Real >::setSampleSpan( int idx , int& start , int& end , double smooth ) const
template<int Degree,class Real>
void BSplineData<Degree,Real>::clearValueTables(void){
- if( valueTables){delete[] valueTables;}
- if(dValueTables){delete[] dValueTables;}
+ delete[] valueTables;
+ delete[] dValueTables;
valueTables=dValueTables=NULL;
}
POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "B-spline up-sampling not supported for degree " << Degree);
}
template<>
- void BSplineElements< 1 >::upSample( BSplineElements< 1 >& high ) const
- {
- high.resize( size()*2 );
- high.assign( high.size() , BSplineElementCoefficients<1>() );
- for( int i=0 ; i<int(size()) ; i++ )
- {
- high[2*i+0][0] += 1 * (*this)[i][0];
- high[2*i+0][1] += 0 * (*this)[i][0];
- high[2*i+1][0] += 2 * (*this)[i][0];
- high[2*i+1][1] += 1 * (*this)[i][0];
+ void BSplineElements< 1 >::upSample( BSplineElements< 1 >& high ) const;
- high[2*i+0][0] += 1 * (*this)[i][1];
- high[2*i+0][1] += 2 * (*this)[i][1];
- high[2*i+1][0] += 0 * (*this)[i][1];
- high[2*i+1][1] += 1 * (*this)[i][1];
- }
- high.denominator = denominator * 2;
- }
template<>
- void BSplineElements< 2 >::upSample( BSplineElements< 2 >& high ) const
- {
- // /----\
- // / \
- // / \ = 1 /--\ +3 /--\ +3 /--\ +1 /--\
- // / \ / \ / \ / \ / \
- // |----------| |----------| |----------| |----------| |----------|
-
- high.resize( size()*2 );
- high.assign( high.size() , BSplineElementCoefficients<2>() );
- for( int i=0 ; i<int(size()) ; i++ )
- {
- high[2*i+0][0] += 1 * (*this)[i][0];
- high[2*i+0][1] += 0 * (*this)[i][0];
- high[2*i+0][2] += 0 * (*this)[i][0];
- high[2*i+1][0] += 3 * (*this)[i][0];
- high[2*i+1][1] += 1 * (*this)[i][0];
- high[2*i+1][2] += 0 * (*this)[i][0];
-
- high[2*i+0][0] += 3 * (*this)[i][1];
- high[2*i+0][1] += 3 * (*this)[i][1];
- high[2*i+0][2] += 1 * (*this)[i][1];
- high[2*i+1][0] += 1 * (*this)[i][1];
- high[2*i+1][1] += 3 * (*this)[i][1];
- high[2*i+1][2] += 3 * (*this)[i][1];
-
- high[2*i+0][0] += 0 * (*this)[i][2];
- high[2*i+0][1] += 1 * (*this)[i][2];
- high[2*i+0][2] += 3 * (*this)[i][2];
- high[2*i+1][0] += 0 * (*this)[i][2];
- high[2*i+1][1] += 0 * (*this)[i][2];
- high[2*i+1][2] += 1 * (*this)[i][2];
- }
- high.denominator = denominator * 4;
- }
+ void BSplineElements< 2 >::upSample( BSplineElements< 2 >& high ) const;
template< int Degree >
void BSplineElements< Degree >::differentiate( BSplineElements< Degree-1 >& d ) const
{
if(res)
{
- if( dotTable) delete[] dotTable;
- if( dDotTable) delete[] dDotTable;
- if(d2DotTable) delete[] d2DotTable;
- if( valueTables) delete[] valueTables;
- if(dValueTables) delete[] dValueTables;
+ delete[] dotTable;
+ delete[] dDotTable;
+ delete[] d2DotTable;
+ delete[] valueTables;
+ delete[] dValueTables;
}
dotTable=dDotTable=d2DotTable=NULL;
valueTables=dValueTables=NULL;
template<int Degree,class Real>
void FunctionData<Degree,Real>::clearDotTables( const int& flags )
{
- if((flags & DOT_FLAG) && dotTable)
+ if (flags & DOT_FLAG)
{
delete[] dotTable;
dotTable=NULL;
- }
- if((flags & D_DOT_FLAG) && dDotTable)
- {
delete[] dDotTable;
dDotTable=NULL;
- }
- if((flags & D2_DOT_FLAG) && d2DotTable)
- {
delete[] d2DotTable;
d2DotTable=NULL;
}
template<int Degree,class Real>
void FunctionData<Degree,Real>::clearValueTables(void){
- if( valueTables){delete[] valueTables;}
- if(dValueTables){delete[] dValueTables;}
+ delete[] valueTables;
+ delete[] dValueTables;
valueTables=dValueTables=NULL;
}
Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
-in the documentation and/or other materials provided with the distribution.
+in the documentation and/or other materials provided with the distribution.
Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
-prior written permission.
+prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
-EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
{
namespace poisson
{
-
-
template<class Real>
Real Random(void){return Real(rand())/RAND_MAX;}
p.coords[1]=-p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0];
p.coords[2]= p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0];
}
+
template<class Real>
void EdgeCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){
int i,j,*remapTable,*pointCount,idx[3];
delete[] pointCount;
delete[] remapTable;
}
+
template<class Real>
void TriangleCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){
int i,j,*remapTable,*pointCount,idx[3];
else {p3=edges[triangles[tIndex].eIndex[2]].pIndex[1];}
return 1;
}
+
template<class Real>
double Triangulation<Real>::area(int p1,int p2,int p3){
Point3D<Real> q1,q2,q;
CrossProduct(q1,q2,q);
return Length(q);
}
+
template<class Real>
double Triangulation<Real>::area(int tIndex){
int p1,p2,p3;
factor(tIndex,p1,p2,p3);
return area(p1,p2,p3);
}
+
template<class Real>
double Triangulation<Real>::area(void){
double a=0;
for(int i=0;i<int(triangles.size());i++){a+=area(i);}
return a;
}
+
template<class Real>
int Triangulation<Real>::addTriangle(int p1,int p2,int p3){
int tIdx,eIdx,p[3];
}
return tIdx;
}
+
template<class Real>
int Triangulation<Real>::flipMinimize(int eIndex){
double oldArea,newArea;
}
return 1;
}
-
}
}
template <class Real>
MinimalAreaTriangulation<Real>::~MinimalAreaTriangulation(void)
{
- if(bestTriangulation)
delete[] bestTriangulation;
bestTriangulation=NULL;
- if(midPoint)
delete[] midPoint;
midPoint=NULL;
}
}
return;
}
- if(bestTriangulation)
delete[] bestTriangulation;
- if(midPoint)
delete[] midPoint;
bestTriangulation=NULL;
midPoint=NULL;
template <class Real>
Real MinimalAreaTriangulation<Real>::GetArea(const std::vector<Point3D<Real> >& vertices)
{
- if(bestTriangulation)
delete[] bestTriangulation;
- if(midPoint)
delete[] midPoint;
bestTriangulation=NULL;
midPoint=NULL;
Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
-in the documentation and/or other materials provided with the distribution.
+in the documentation and/or other materials provided with the distribution.
Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
-prior written permission.
+prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
-EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
maxDepth=0;
}
SortedTreeNodes::~SortedTreeNodes(void){
- if( nodeCount ) delete[] nodeCount;
- if( treeNodes ) delete[] treeNodes;
+ delete[] nodeCount;
+ delete[] treeNodes;
nodeCount = NULL;
treeNodes = NULL;
}
void SortedTreeNodes::set( TreeOctNode& root )
{
- if( nodeCount ) delete[] nodeCount;
- if( treeNodes ) delete[] treeNodes;
+ delete[] nodeCount;
+ delete[] treeNodes;
maxDepth = root.maxDepth()+1;
nodeCount = new int[ maxDepth+1 ];
treeNodes = new TreeOctNode*[ root.nodes() ];
}
template< int Degree >
int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 ) const { return GetMatrixRowSize( neighbors5 , 0 , 5 , 0 , 5 , 0 , 5 ); }
+
template< int Degree >
int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const
{
count++;
return count;
}
+
template< int Degree >
int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const
{
return SetMatrixRow( neighbors5 , row , offset , stencil , 0 , 5 , 0 , 5 , 0 , 5 );
}
+
template< int Degree >
int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const
{
}
return count;
}
+
template< int Degree >
void Octree< Degree >::SetDivergenceStencil( int depth , Point3D< double > *stencil , bool scatter ) const
{
#endif // GRADIENT_DOMAIN_SOLUTION
}
}
+
template< int Degree >
void Octree< Degree >::SetLaplacianStencil( int depth , double stencil[5][5][5] ) const
{
stencil[x][y][z] = GetLaplacian( symIndex );
}
}
+
template< int Degree >
void Octree< Degree >::SetLaplacianStencils( int depth , Stencil< double , 5 > stencils[2][2][2] ) const
{
}
}
}
+
template< int Degree >
void Octree< Degree >::SetDivergenceStencils( int depth , Stencil< Point3D< double > , 5 > stencils[2][2][2] , bool scatter ) const
{
}
}
}
+
template< int Degree >
void Octree< Degree >::SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const
{
}
}
}
+
template< int Degree >
void Octree< Degree >::UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ )
{
UpSampleData( void ) { start = 0 , v[0] = v[1] = 0.; }
UpSampleData( int s , double v1 , double v2 ) { start = s , v[0] = v1 , v[1] = v2; }
};
+
template< int Degree >
void Octree< Degree >::UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , Vector< Real >& Solution ) const
{
constraints[i] += cSum;
}
}
+
template< int Degree >
template< class C >
void Octree< Degree >::UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const
}
}
}
+
template< int Degree >
void Octree< Degree >::SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution )
{
}
}
}
+
template< int Degree >
Real Octree< Degree >::WeightedCoarserFunctionValue( const OctNode< TreeNodeData , Real >::NeighborKey3& neighborKey , const TreeOctNode* pointNode , Real* metSolution ) const
{
}
return Real( pointValue * weight );
}
+
template<int Degree>
int Octree<Degree>::GetFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix , int depth , const SortedTreeNodes& sNodes , Real* metSolution )
{
}
return 1;
}
+
template<int Degree>
int Octree<Degree>::GetRestrictedFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix,int depth,const int* entries,int entryCount,
const TreeOctNode* rNode , Real radius ,
return iter;
}
+
template<int Degree>
int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , int startingDepth , bool showResidual , int minIters , double accuracy )
{
delete[] asf.adjacencies;
return tIter;
}
+
template<int Degree>
int Octree<Degree>::HasNormals(TreeOctNode* node,Real epsilon)
{
return hasNormals;
}
+
template<int Degree>
void Octree<Degree>::ClipTree( void )
{
}
MemoryUsage();
}
+
template<int Degree>
void Octree<Degree>::SetLaplacianConstraints( void )
{
delete normals;
normals = NULL;
}
+
template<int Degree>
void Octree<Degree>::AdjacencyCountFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencyCount++;}
+
template<int Degree>
void Octree<Degree>::AdjacencySetFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencies[adjacencyCount++]=node1->nodeData.nodeIndex;}
+
template<int Degree>
void Octree<Degree>::RefineFunction::Function( TreeOctNode* node1 , const TreeOctNode* node2 )
{
if( !node1->children && node1->depth()<depth ) node1->initChildren();
}
+
template< int Degree >
void Octree< Degree >::FaceEdgesFunction::Function( const TreeOctNode* node1 , const TreeOctNode* node2 )
{
_sNodes.set( tree );
MemoryUsage();
}
+
template<int Degree>
void Octree<Degree>::GetMCIsoTriangles( Real isoValue , int subdivideDepth , CoredMeshData* mesh , int fullDepthIso , int nonLinearFit , bool addBarycenter , bool polygonMesh )
{
delete[] coarseRootData.cornerValuesSet , delete[] coarseRootData.cornerNormalsSet;
delete rootData.boundaryValues;
}
+
template<int Degree>
Real Octree<Degree>::getCenterValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey , const TreeOctNode* node){
int idx[3];
}
return value;
}
+
template< int Degree >
Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution )
{
}
return Real( value );
}
+
template< int Degree >
Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution , const double stencil1[3][3][3] , const double stencil2[3][3][3] )
{
}
return Real( value );
}
+
template< int Degree >
Point3D< Real > Octree< Degree >::getCornerNormal( const OctNode< TreeNodeData , Real >::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution )
{
}
return normal;
}
+
template< int Degree >
Real Octree<Degree>::GetIsoValue( void )
{
}
}
-
template<int Degree>
int Octree<Degree>::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,int maxDepth){
int c1,c2,e1,e2,dir,off,cnt=0;
if(finest->children) return EdgeRootCount(&finest->children[c1],eIndex,maxDepth)+EdgeRootCount(&finest->children[c2],eIndex,maxDepth);
else return MarchingCubes::HasEdgeRoots(finest->nodeData.mcIndex,eIndex);
}
+
template<int Degree>
int Octree<Degree>::IsBoundaryFace(const TreeOctNode* node,int faceIndex,int subdivideDepth){
int dir,offset,d,o[3],idx;
idx=(int(o[dir])<<1) + (offset<<1);
return !(idx%(2<<(int(node->d)-subdivideDepth)));
}
+
template<int Degree>
int Octree<Degree>::IsBoundaryEdge(const TreeOctNode* node,int edgeIndex,int subdivideDepth){
int dir,x,y;
Cube::FactorEdgeIndex(edgeIndex,dir,x,y);
return IsBoundaryEdge(node,dir,x,y,subdivideDepth);
}
+
template<int Degree>
int Octree<Degree>::IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subdivideDepth )
{
mask = 1<<( int(node->d) - subdivideDepth );
return !(idx1%(mask)) || !(idx2%(mask));
}
+
template< int Degree >
void Octree< Degree >::GetRootSpan( const RootInfo& ri , Point3D< float >& start , Point3D< float >& end )
{
break;
}
}
+
//////////////////////////////////////////////////////////////////////////////////////
// The assumption made when calling this code is that the edge has at most one root //
//////////////////////////////////////////////////////////////////////////////////////
position[o] = Real(center-width/2+width*averageRoot);
return 1;
}
+
template< int Degree >
int Octree< Degree >::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth,RootInfo& ri )
{
return 1;
}
}
+
template<int Degree>
int Octree<Degree>::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri )
{
return 1;
}
}
+
template<int Degree>
int Octree<Degree>::GetRootPair(const RootInfo& ri,int maxDepth,RootInfo& pair){
const TreeOctNode* node=ri.node;
node=node->parent;
}
return 0;
-
}
+
template<int Degree>
int Octree< Degree >::GetRootIndex( const RootInfo& ri , RootData& rootData , CoredPointIndex& index )
{
}
return 0;
}
+
template< int Degree >
int Octree< Degree >::SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData ,
std::vector< Point3D< float > >* interiorPositions , CoredMeshData* mesh , const Real* metSolution , int nonLinearFit )
}
return count;
}
+
template<int Degree>
int Octree< Degree >::SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , CoredMeshData* mesh , int nonLinearFit )
{
}
return count;
}
+
template<int Degree>
void Octree< Degree >::GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges )
{
}
}
}
+
template<int Degree>
#if MISHA_DEBUG
int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool polygonMesh , std::vector< Point3D< float > >* barycenters )
}
return int(loops.size());
}
+
template<int Degree>
#if MISHA_DEBUG
int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool polygonMesh , std::vector< Point3D< float > >* barycenters )
}
return int(edges.size())-2;
}
+
template< int Degree >
Real* Octree< Degree >::GetSolutionGrid( int& res , float isoValue , int depth )
{
return values;
}
+
template< int Degree >
Real* Octree< Degree >::GetWeightGrid( int& res , int depth )
{
int idx[DIMENSION];
return CenterIndex(node,maxDepth,idx);
}
+
long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION]){
int d,o[3];
node->depthAndOffset(d,o);
for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
+
long long VertexData::CenterIndex(int depth,const int offSet[DIMENSION],int maxDepth,int idx[DIMENSION]){
for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,depth+1,offSet[i]<<1,1);}
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
+
long long VertexData::CornerIndex(const TreeOctNode* node,int cIndex,int maxDepth){
int idx[DIMENSION];
return CornerIndex(node,cIndex,maxDepth,idx);
}
+
long long VertexData::CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int idx[DIMENSION] )
{
int x[DIMENSION];
for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , d , o[i] , x[i] );
return CornerIndexKey( idx );
}
+
long long VertexData::CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int idx[DIMENSION] )
{
int x[DIMENSION];
for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , depth , offSet[i] , x[i] );
return CornerIndexKey( idx );
}
+
long long VertexData::CornerIndexKey( const int idx[DIMENSION] )
{
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
+
long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth){
int idx[DIMENSION];
return FaceIndex(node,fIndex,maxDepth,idx);
}
+
long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth,int idx[DIMENSION]){
int dir,offset;
Cube::FactorFaceIndex(fIndex,dir,offset);
idx[dir]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,o[dir],offset);
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
+
long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth){
int idx[DIMENSION];
return EdgeIndex(node,eIndex,maxDepth,idx);
}
+
long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth,int idx[DIMENSION]){
int o,i1,i2;
int d,off[3];
};
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
-
-
}
}
-
-
Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
-in the documentation and/or other materials provided with the distribution.
+in the documentation and/or other materials provided with the distribution.
Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
-prior written permission.
+prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
-EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
{
namespace poisson
{
-
template<class NodeData,class Real> const int OctNode<NodeData,Real>::DepthShift=5;
template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift=19;
template<class NodeData,class Real> const int OctNode<NodeData,Real>::DepthMask=(1<<DepthShift)-1;
}
else{UseAlloc=0;}
}
+
template<class NodeData,class Real>
int OctNode<NodeData,Real>::UseAllocator(void){return UseAlloc;}
template <class NodeData,class Real>
OctNode<NodeData,Real>::~OctNode(void){
- if(!UseAlloc){if(children){delete[] children;}}
+ if(!UseAlloc){delete[] children;}
parent=children=NULL;
}
+
template <class NodeData,class Real>
void OctNode<NodeData,Real>::setFullDepth(int maxDepth){
if( maxDepth )
if(UseAlloc){children=internalAllocator.newElements(8);}
else{
- if(children){delete[] children;}
+ delete[] children;
children=NULL;
children=new OctNode[Cube::CORNERS];
}
}
return 1;
}
+
template <class NodeData,class Real>
inline void OctNode<NodeData,Real>::Index(int depth,const int offset[3],short& d,short off[3]){
d=short(depth);
offset[1]=(int(off[1])+1)&(~(1<<depth));
offset[2]=(int(off[2])+1)&(~(1<<depth));
}
+
template<class NodeData,class Real>
inline int OctNode<NodeData,Real>::depth(void) const {return int(d);}
template<class NodeData,class Real>
offset[1]=(int((index>>OffsetShift2)&OffsetMask)+1)&(~(1<<depth));
offset[2]=(int((index>>OffsetShift3)&OffsetMask)+1)&(~(1<<depth));
}
+
template<class NodeData,class Real>
inline int OctNode<NodeData,Real>::Depth(const long long& index){return int(index&DepthMask);}
template <class NodeData,class Real>
width=Real(1.0/(1<<depth));
for(int dim=0;dim<DIMENSION;dim++){center.coords[dim]=Real(0.5+offset[dim])*width;}
}
+
template< class NodeData , class Real >
bool OctNode< NodeData , Real >::isInside( Point3D< Real > p ) const
{
w /= 2;
return (c[0]-w)<p[0] && p[0]<=(c[0]+w) && (c[1]-w)<p[1] && p[1]<=(c[1]+w) && (c[2]-w)<p[2] && p[2]<=(c[2]+w);
}
+
template <class NodeData,class Real>
inline void OctNode<NodeData,Real>::CenterAndWidth(const long long& index,Point3D<Real>& center,Real& width){
int depth,offset[3];
return c+1;
}
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::nodes(void) const{
if(!children){return 1;}
return c+1;
}
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::leaves(void) const{
if(!children){return 1;}
return c;
}
}
+
template<class NodeData,class Real>
int OctNode<NodeData,Real>::maxDepthLeaves(int maxDepth) const{
if(depth()>maxDepth){return 0;}
return c;
}
}
+
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::root(void) const{
const OctNode* temp=this;
return temp;
}
-
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextBranch( const OctNode* current ) const
{
if(current-current->parent->children==Cube::CORNERS-1) return nextBranch( current->parent );
else return current+1;
}
+
template <class NodeData,class Real>
OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextBranch(OctNode* current){
if(!current->parent || current==this){return NULL;}
if(current-current->parent->children==Cube::CORNERS-1){return nextBranch(current->parent);}
else{return current+1;}
}
+
template< class NodeData , class Real >
const OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( const OctNode* current ) const
{
if( current-current->parent->children==0 ) return prevBranch( current->parent );
else return current-1;
}
+
template< class NodeData , class Real >
OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( OctNode* current )
{
if( current-current->parent->children==0 ) return prevBranch( current->parent );
else return current-1;
}
+
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextLeaf(const OctNode* current) const{
if(!current){
if(!temp){return NULL;}
else{return temp->nextLeaf();}
}
+
template <class NodeData,class Real>
OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextLeaf(OctNode* current){
if(!current){
else if( current->children ) return ¤t->children[0];
else return nextBranch(current);
}
+
template <class NodeData,class Real>
OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextNode( OctNode* current )
{
if(processCurrent){F->Function(this,node);}
if(children){__processNodeNodes(node,F);}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int fIndex,int processCurrent){
__processNodeFaces(node,F,c1,c2,c3,c4);
}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int eIndex,int processCurrent){
__processNodeEdges(node,F,c1,c2);
}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::processNodeCorners(OctNode* node,NodeAdjacencyFunction* F,int cIndex,int processCurrent){
F->Function(temp,node);
}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::__processNodeNodes(OctNode* node,NodeAdjacencyFunction* F){
if(children[6].children){children[6].__processNodeNodes(node,F);}
if(children[7].children){children[7].__processNodeNodes(node,F);}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::__processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2){
if(children[cIndex1].children){children[cIndex1].__processNodeEdges(node,F,cIndex1,cIndex2);}
if(children[cIndex2].children){children[cIndex2].__processNodeEdges(node,F,cIndex1,cIndex2);}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::__processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2,int cIndex3,int cIndex4){
if(children[cIndex3].children){children[cIndex3].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);}
if(children[cIndex4].children){children[cIndex4].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);}
}
+
template<class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,NodeAdjacencyFunction* F,int processCurrent){
ProcessNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent);
}
+
template<class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessNodeAdjacentNodes(int dx,int dy,int dz,
if(!node2->children){return;}
__ProcessNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F);
}
+
template<class NodeData,class Real>
template<class TerminatingNodeAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessTerminatingNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent){
ProcessTerminatingNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent);
}
+
template<class NodeData,class Real>
template<class TerminatingNodeAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz,
if(!node2->children){return;}
__ProcessTerminatingNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F);
}
+
template<class NodeData,class Real>
template<class PointAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessPointAdjacentNodes( int maxDepth , const int c1[3] , OctNode* node2 , int width2 , PointAdjacencyFunction* F , int processCurrent )
w2 = node2->width( maxDepth+1 );
ProcessPointAdjacentNodes( c1[0]-c2[0] , c1[1]-c2[1] , c1[2]-c2[2] , node2 , (width2*w2)>>1 , w2 , F , processCurrent );
}
+
template<class NodeData,class Real>
template<class PointAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessPointAdjacentNodes(int dx,int dy,int dz,
if( !node2->children ) return;
__ProcessPointAdjacentNodes( -dx , -dy , -dz , node2 , radius2 , width2>>1 , F );
}
+
template<class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessFixedDepthNodeAdjacentNodes(int maxDepth,
ProcessFixedDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent);
}
+
template<class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz,
__ProcessFixedDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,depth-1,F);
}
}
+
template<class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessMaxDepthNodeAdjacentNodes(int maxDepth,
w2=node2->width(maxDepth+1);
ProcessMaxDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent);
}
+
template<class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz,
if(processCurrent){F->Function(node2,node1);}
if(d<depth && node2->children){__ProcessMaxDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2>>1,depth-1,F);}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::__ProcessNodeAdjacentNodes(int dx,int dy,int dz,
if(o&128){F->Function(&node2->children[7],node1);if(node2->children[7].children){__ProcessNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}}
}
}
+
template <class NodeData,class Real>
template<class TerminatingNodeAdjacencyFunction>
void OctNode<NodeData,Real>::__ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz,
if(o&128){if(F->Function(&node2->children[7],node1) && node2->children[7].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}}
}
}
+
template <class NodeData,class Real>
template<class PointAdjacencyFunction>
void OctNode<NodeData,Real>::__ProcessPointAdjacentNodes(int dx,int dy,int dz,
if(o&128){F->Function(&node2->children[7]);if(node2->children[7].children){__ProcessPointAdjacentNodes(dx2,dy2,dz2,&node2->children[7],radius,cWidth,F);}}
}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::__ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz,
}
}
}
+
template <class NodeData,class Real>
template<class NodeAdjacencyFunction>
void OctNode<NodeData,Real>::__ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz,
}
}
}
+
template <class NodeData,class Real>
inline int OctNode<NodeData,Real>::ChildOverlap(int dx,int dy,int dz,int d,int cRadius2)
{
}
return temp;
}
+
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::getNearestLeaf(const Point3D<Real>& p) const{
int nearest;
if(idx1[0]==idx2[0] && idx1[1]==idx2[1]){return 1;}
else {return 0;}
}
+
template<class NodeData,class Real>
int OctNode<NodeData,Real>::CornerIndex(const Point3D<Real>& center,const Point3D<Real>& p){
int cIndex=0;
if(p.coords[2]>center.coords[2]){cIndex|=4;}
return cIndex;
}
+
template <class NodeData,class Real>
template<class NodeData2>
OctNode<NodeData,Real>& OctNode<NodeData,Real>::operator = (const OctNode<NodeData2,Real>& node){
int i;
- if(children){delete[] children;}
+ delete[] children;
children=NULL;
d=node.depth ();
}
return *this;
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::CompareForwardDepths(const void* v1,const void* v2){
return ((const OctNode<NodeData,Real>*)v1)->depth-((const OctNode<NodeData,Real>*)v2)->depth;
}
+
template< class NodeData , class Real >
int OctNode< NodeData , Real >::CompareByDepthAndXYZ( const void* v1 , const void* v2 )
{
for( int i=0 ; i<31 ; i++ ) key |= ( ( _p[0] & (1ull<<i) )<<(2*i) ) | ( ( _p[1] & (1ull<<i) )<<(2*i+1) ) | ( ( _p[2] & (1ull<<i) )<<(2*i+2) );
return key;
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::CompareByDepthAndZIndex( const void* v1 , const void* v2 )
{
return int(n1->off[2])-int(n2->off[2]);
return 0;
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::CompareBackwardDepths(const void* v1,const void* v2){
return ((const OctNode<NodeData,Real>*)v2)->depth-((const OctNode<NodeData,Real>*)v1)->depth;
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::CompareBackwardPointerDepths(const void* v1,const void* v2){
return (*(const OctNode<NodeData,Real>**)v2)->depth()-(*(const OctNode<NodeData,Real>**)v1)->depth();
}
+
template <class NodeData,class Real>
inline int OctNode<NodeData,Real>::Overlap2(const int &depth1,const int offSet1[DIMENSION],const Real& multiplier1,const int &depth2,const int offSet2[DIMENSION],const Real& multiplier2){
int d=depth2-depth1;
){return 0;}
return 1;
}
+
template <class NodeData,class Real>
inline int OctNode<NodeData,Real>::Overlap(int c1,int c2,int c3,int dWidth){
if(c1>=dWidth || c1<=-dWidth || c2>=dWidth || c2<=-dWidth || c3>=dWidth || c3<=-dWidth){return 0;}
else{return 1;}
}
+
template <class NodeData,class Real>
OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex,int forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);}
template <class NodeData,class Real>
return &temp->children[pIndex];
}
}
+
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off) const {
if(!parent){return NULL;}
};
return __edgeNeighbor(o,i,idx,forceChildren);
}
+
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::edgeNeighbor(int edgeIndex) const {
int idx[2],o,i[2];
};
return __edgeNeighbor(o,i,idx);
}
+
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{
if(!parent){return NULL;}
}
else{return NULL;}
}
+
template <class NodeData,class Real>
OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){
if(!parent){return NULL;}
}
else{return NULL;}
}
+
template <class NodeData,class Real>
OctNode<NodeData,Real>* OctNode<NodeData,Real>::cornerNeighbor(int cornerIndex,int forceChildren){
int pIndex,aIndex=0;
}
else{return NULL;}
}
+
////////////////////////
// OctNodeNeighborKey //
////////////////////////
template<class NodeData,class Real>
OctNode<NodeData,Real>::Neighbors3::Neighbors3(void){clear();}
+
template<class NodeData,class Real>
void OctNode<NodeData,Real>::Neighbors3::clear(void){
for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}}
}
+
template<class NodeData,class Real>
OctNode<NodeData,Real>::NeighborKey3::NeighborKey3(void){ neighbors=NULL; }
+
template<class NodeData,class Real>
OctNode<NodeData,Real>::NeighborKey3::~NeighborKey3(void)
{
- if( neighbors ) delete[] neighbors;
+ delete[] neighbors;
neighbors = NULL;
}
template<class NodeData,class Real>
void OctNode<NodeData,Real>::NeighborKey3::set( int d )
{
- if( neighbors ) delete[] neighbors;
+ delete[] neighbors;
neighbors = NULL;
if( d<0 ) return;
neighbors = new Neighbors3[d+1];
}
+
template< class NodeData , class Real >
typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* root , Point3D< Real > p , int d )
{
}
return neighbors[d];
}
+
template< class NodeData , class Real >
typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::getNeighbors( OctNode<NodeData,Real>* root , Point3D< Real > p , int d )
{
}
return neighbors[d];
}
+
// Note the assumption is that if you enable an edge, you also enable adjacent faces.
// And, if you enable a corner, you enable adjacent edges and faces.
template< class NodeData , class Real >
///////////////////////
template<class NodeData,class Real>
OctNode<NodeData,Real>::ConstNeighbors3::ConstNeighbors3(void){clear();}
+
template<class NodeData,class Real>
void OctNode<NodeData,Real>::ConstNeighbors3::clear(void){
for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}}
}
+
template<class NodeData,class Real>
OctNode<NodeData,Real>::ConstNeighborKey3::ConstNeighborKey3(void){neighbors=NULL;}
+
template<class NodeData,class Real>
OctNode<NodeData,Real>::ConstNeighborKey3::~ConstNeighborKey3(void){
- if(neighbors){delete[] neighbors;}
+ delete[] neighbors;
neighbors=NULL;
}
template<class NodeData,class Real>
void OctNode<NodeData,Real>::ConstNeighborKey3::set(int d){
- if(neighbors){delete[] neighbors;}
+ delete[] neighbors;
neighbors=NULL;
if(d<0){return;}
neighbors=new ConstNeighbors3[d+1];
}
+
template<class NodeData,class Real>
typename OctNode<NodeData,Real>::ConstNeighbors3& OctNode<NodeData,Real>::ConstNeighborKey3::getNeighbors(const OctNode<NodeData,Real>* node){
int d=node->depth();
}
return neighbors[node->depth()];
}
+
template<class NodeData,class Real>
typename OctNode<NodeData,Real>::ConstNeighbors3& OctNode<NodeData,Real>::ConstNeighborKey3::getNeighbors( const OctNode<NodeData,Real>* node , int minDepth )
{
}
template< class NodeData , class Real > OctNode< NodeData , Real >::Neighbors5::Neighbors5( void ){ clear(); }
+
template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighbors5::ConstNeighbors5( void ){ clear(); }
+
template< class NodeData , class Real >
void OctNode< NodeData , Real >::Neighbors5::clear( void )
{
for( int i=0 ; i<5 ; i++ ) for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) neighbors[i][j][k] = NULL;
}
+
template< class NodeData , class Real >
void OctNode< NodeData , Real >::ConstNeighbors5::clear( void )
{
for( int i=0 ; i<5 ; i++ ) for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) neighbors[i][j][k] = NULL;
}
+
template< class NodeData , class Real >
OctNode< NodeData , Real >::NeighborKey5::NeighborKey5( void )
{
_depth = -1;
neighbors = NULL;
}
+
template< class NodeData , class Real >
OctNode< NodeData , Real >::ConstNeighborKey5::ConstNeighborKey5( void )
{
_depth = -1;
neighbors = NULL;
}
+
template< class NodeData , class Real >
OctNode< NodeData , Real >::NeighborKey5::~NeighborKey5( void )
{
- if( neighbors ) delete[] neighbors;
+ delete[] neighbors;
neighbors = NULL;
}
+
template< class NodeData , class Real >
OctNode< NodeData , Real >::ConstNeighborKey5::~ConstNeighborKey5( void )
{
- if( neighbors ) delete[] neighbors;
+ delete[] neighbors;
neighbors = NULL;
}
+
template< class NodeData , class Real >
void OctNode< NodeData , Real >::NeighborKey5::set( int d )
{
- if( neighbors ) delete[] neighbors;
+ delete[] neighbors;
neighbors = NULL;
if(d<0) return;
_depth = d;
neighbors=new Neighbors5[d+1];
}
+
template< class NodeData , class Real >
void OctNode< NodeData , Real >::ConstNeighborKey5::set( int d )
{
- if( neighbors ) delete[] neighbors;
+ delete[] neighbors;
neighbors = NULL;
if(d<0) return;
_depth = d;
neighbors=new ConstNeighbors5[d+1];
}
+
template< class NodeData , class Real >
typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::getNeighbors( OctNode* node )
{
}
return neighbors[d];
}
+
template< class NodeData , class Real >
typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::setNeighbors( OctNode* node , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd )
{
}
return neighbors[d];
}
+
template< class NodeData , class Real >
typename OctNode< NodeData , Real >::ConstNeighbors5& OctNode< NodeData , Real >::ConstNeighborKey5::getNeighbors( const OctNode* node )
{
return neighbors[d];
}
-
template <class NodeData,class Real>
int OctNode<NodeData,Real>::write(const char* fileName) const{
FILE* fp=fopen(fileName,"wb");
fclose(fp);
return ret;
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::write(FILE* fp) const{
fwrite(this,sizeof(OctNode<NodeData,Real>),1,fp);
if(children){for(int i=0;i<Cube::CORNERS;i++){children[i].write(fp);}}
return 1;
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::read(const char* fileName){
FILE* fp=fopen(fileName,"rb");
fclose(fp);
return ret;
}
+
template <class NodeData,class Real>
int OctNode<NodeData,Real>::read(FILE* fp){
fread(this,sizeof(OctNode<NodeData,Real>),1,fp);
}
return 1;
}
+
template<class NodeData,class Real>
int OctNode<NodeData,Real>::width(int maxDepth) const {
int d=depth();
return 1<<(maxDepth-d);
}
+
template<class NodeData,class Real>
void OctNode<NodeData,Real>::centerIndex(int maxDepth,int index[DIMENSION]) const {
int d,o[3];
depthAndOffset(d,o);
for(int i=0;i<DIMENSION;i++){index[i]=BinaryNode<Real>::CornerIndex(maxDepth,d+1,o[i]<<1,1);}
}
-
}
}
Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
-in the documentation and/or other materials provided with the distribution.
+in the documentation and/or other materials provided with the distribution.
Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
-prior written permission.
+prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
-EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
DAMAGE.
*/
+#include "factor.h"
+
#include <float.h>
#include <math.h>
+
#include <algorithm>
-#include "factor.h"
+
+#include <cstdio>
+#include <cstring>
////////////////
// Polynomial //
for(int i=0;i<=Degree;i++){p.coefficients[i+1]=coefficients[i]/(i+1);}
return p;
}
- template<> double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; }
- template<> double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; }
- template<> double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; }
+ template<> inline double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; }
+ template<> inline double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; }
+ template<> inline double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; }
template<int Degree>
double Polynomial<Degree>::operator() ( double t ) const{
double v=coefficients[Degree];
}
}
}
- template< >
+ template< > inline
Polynomial< 0 > Polynomial< 0 >::BSplineComponent( int i )
{
Polynomial p;
p.coefficients[0] = 1.;
return p;
}
- template< int Degree >
+ template< int Degree > inline
Polynomial< Degree > Polynomial< Degree >::BSplineComponent( int i )
{
Polynomial p;
#include "factor.h"
+#include <cstdio>
+#include <cstring>
+
////////////////////////
// StartingPolynomial //
////////////////////////
}
printf("\n");
}
- template< >
+ template< > inline
PPolynomial< 0 > PPolynomial< 0 >::BSpline( double radius )
{
PPolynomial q;
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/surface/processing.h>
#pragma once
#if defined __GNUC__
-# pragma GCC system_header
+# pragma GCC system_header
#endif
#include <boost/dynamic_bitset/dynamic_bitset.hpp>
-#include <boost/shared_ptr.hpp>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/pcl_config.h>
#ifdef HAVE_QHULL
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/surface/boost.h>
#include <pcl/surface/reconstruction.h>
#ifndef PCL_SURFACE_IMPL_MLS_H_
#define PCL_SURFACE_IMPL_MLS_H_
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
#include <pcl/surface/mls.h>
#include <pcl/common/io.h>
#include <pcl/common/copy_point.h>
#endif
// For all points
-#ifdef _OPENMP
-#pragma omp parallel for schedule (dynamic,1000) num_threads (threads)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(corresponding_input_indices, projected_points, projected_points_normals) \
+ schedule(dynamic,1000) \
+ num_threads(threads)
for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
{
// Allocate enough space to hold the results of nearest neighbor searches
*
*/
+#pragma once
+
+
+namespace pcl
+{
-//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
-
pcl::CloudSurfaceProcessing<PointInT, PointOutT>::process (pcl::PointCloud<PointOutT> &output)
+CloudSurfaceProcessing<PointInT, PointOutT>::process (pcl::PointCloud<PointOutT> &output)
{
// Copy the header
output.header = input_->header;
deinitCompute ();
}
+
+} // namespace pcl
+
#ifndef PCL_SURFACE_RECONSTRUCTION_IMPL_H_
#define PCL_SURFACE_RECONSTRUCTION_IMPL_H_
+
#include <pcl/search/pcl_search.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template <typename PointInT> void
-
pcl::SurfaceReconstruction<PointInT>::reconstruct (pcl::PolygonMesh &output)
+SurfaceReconstruction<PointInT>::reconstruct (pcl::PolygonMesh &output)
{
// Copy the header
output.header = input_->header;
- if (!initCompute ())
+ if (!initCompute ())
{
output.cloud.width = output.cloud.height = 0;
output.cloud.data.clear ();
deinitCompute ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT> void
-
pcl::SurfaceReconstruction<PointInT>::reconstruct (pcl::PointCloud<PointInT> &points,
-
std::vector<pcl::Vertices> &polygons)
+SurfaceReconstruction<PointInT>::reconstruct (pcl::PointCloud<PointInT> &points,
+ std::vector<pcl::Vertices> &polygons)
{
// Copy the header
points.header = input_->header;
- if (!initCompute ())
+ if (!initCompute ())
{
points.width = points.height = 0;
points.clear ();
deinitCompute ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT> void
-
pcl::MeshConstruction<PointInT>::reconstruct (pcl::PolygonMesh &output)
+MeshConstruction<PointInT>::reconstruct (pcl::PolygonMesh &output)
{
// Copy the header
output.header = input_->header;
- if (!initCompute ())
+ if (!initCompute ())
{
output.cloud.width = output.cloud.height = 1;
output.cloud.data.clear ();
deinitCompute ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT> void
-
pcl::MeshConstruction<PointInT>::reconstruct (std::vector<pcl::Vertices> &polygons)
+MeshConstruction<PointInT>::reconstruct (std::vector<pcl::Vertices> &polygons)
{
- if (!initCompute ())
+ if (!initCompute ())
{
polygons.clear ();
return;
deinitCompute ();
}
+} // namespace pcl
#endif // PCL_SURFACE_RECONSTRUCTION_IMPL_H_
#include <pcl/common/distances.h>
#include <pcl/surface/texture_mapping.h>
+#include <unordered_set>
///////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
visible_pts = *filtered_cloud;
+ // pushing occluded idxs into a set for faster lookup
+ std::unordered_set<index_t> occluded_set(occluded.cbegin(), occluded.cend());
+
// find visible faces => add them to polygon N for camera N
// add polygon group for current camera in clean
std::vector<pcl::Vertices> visibleFaces_currentCam;
for (std::size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces)
{
// check if all the face's points are visible
- bool faceIsVisible = true;
- std::vector<int>::iterator it;
-
// iterate over face's vertex
- for (std::size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice)
+ const auto faceIsVisible = std::all_of(tex_mesh.tex_polygons[0][faces].vertices.cbegin(),
+ tex_mesh.tex_polygons[0][faces].vertices.cend(),
+ [&](const auto& vertex)
{
- // TODO this is far too long! Better create an helper function that raycasts here.
- it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]);
-
- if (it == occluded.end ())
- {
- // point is not occluded
- // does it land on the camera's image plane?
- PointInT pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]];
- Eigen::Vector2f dummy_UV;
- if (!getPointUVCoordinates (pt, camera, dummy_UV))
- {
- // point is not visible by the camera
- faceIsVisible = false;
+ if (occluded_set.find(vertex) != occluded_set.cend()) {
+ return false; // point is occluded
}
- }
- else
- {
- faceIsVisible = false;
- }
- }
+ // is the point visible to the camera?
+ Eigen::Vector2f dummy_UV;
+ return this->getPointUVCoordinates ((*transformed_cloud)[vertex], camera, dummy_UV);
+ });
if (faceIsVisible)
{
template class PCL_EXPORTS pcl::TextureMapping<T>;
#endif /* TEXTURE_MAPPING_HPP_ */
-
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/surface/boost.h>
#include <pcl/surface/reconstruction.h>
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/surface/boost.h>
#include <pcl/surface/marching_cubes.h>
/** \brief Method that sets the distance for ignoring voxels which are far from point cloud.
* If the distance is negative, then the distance functions would be calculated in all voxels;
* otherwise, only voxels with distance lower than dist_ignore would be involved in marching cube.
- * \param[in] threshold of distance. Default value is -1.0. Set to negative if all voxels are
+ * \param[in] dist_ignore threshold of distance. Default value is -1.0. Set to negative if all voxels are
* to be involved.
*/
inline void
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/surface/boost.h>
#include <pcl/surface/marching_cubes.h>
#include <random>
// PCL includes
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/search/pcl_search.h>
/** \brief Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation.
* \param[in] polynomial_fit set to true for polynomial fit
*/
- PCL_DEPRECATED("use setPolynomialOrder() instead")
+ PCL_DEPRECATED(1, 12, "use setPolynomialOrder() instead")
inline void
setPolynomialFit (bool polynomial_fit)
{
}
/** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */
- PCL_DEPRECATED("use getPolynomialOrder() instead")
+ PCL_DEPRECATED(1, 12, "use getPolynomialOrder() instead")
inline bool
getPolynomialFit () const { return (order_ > 1); }
getDilationIterations () const { return (dilation_iteration_num_); }
/** \brief Set whether the mls results should be stored for each point in the input cloud
- * \param[in] True if the mls results should be stored, otherwise false.
+ * \param[in] cache_mls_results True if the mls results should be stored, otherwise false.
* \note The cache_mls_results_ is forced to true when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
* \note If memory consumption is a concern set to false when not using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
*/
};
template <typename PointInT, typename PointOutT>
- using MovingLeastSquaresOMP PCL_DEPRECATED("use MovingLeastSquares instead, it supports OpenMP now") = MovingLeastSquares<PointInT, PointOutT>;
+ using MovingLeastSquaresOMP PCL_DEPRECATED(
1, 12, "use MovingLeastSquares instead, it supports OpenMP now") = MovingLeastSquares<PointInT, PointOutT>;
}
#ifdef PCL_NO_PRECOMPILE
return grc2gl (E + i, F + j);
} // local row/col index to global lexicographic index
int
- gl2gr (int A)
+ gl2gr (int A) const
{
return (A / m_nurbs.CVCount (1));
} // global lexicographic in global row index
int
- gl2gc (int A)
+ gl2gc (int A) const
{
return (A % m_nurbs.CVCount (1));
} // global lexicographic in global col index
return grc2gl (E + i, F + j);
} // local row/col index to global lexicographic index
int
- gl2gr (int A)
+ gl2gr (int A) const
{
return (A / m_nurbs.CVCount (1));
} // global lexicographic in global row index
int
- gl2gc (int A)
+ gl2gc (int A) const
{
return (A % m_nurbs.CVCount (1));
} // global lexicographic in global col index
#include <pcl/surface/on_nurbs/nurbs_data.h>
#include <pcl/surface/on_nurbs/nurbs_solve.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
// index routines
int
- grc2gl (int I, int J)
+ grc2gl (int I, int J) const
{
return m_nurbs.CVCount (1) * I + J;
} // global row/col index to global lexicographic index
return grc2gl (E + i, F + j);
} // local row/col index to global lexicographic index
int
- gl2gr (int A)
+ gl2gr (int A) const
{
return (static_cast<int> (A / m_nurbs.CVCount (1)));
} // global lexicographic in global row index
int
- gl2gc (int A)
+ gl2gc (int A) const
{
return (static_cast<int> (A % m_nurbs.CVCount (1)));
} // global lexicographic in global col index
#undef Success
#include <Eigen/StdVector>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
#undef Success
#include <Eigen/Dense>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/surface/on_nurbs/sparse_mat.h>
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
- *
+ *
*
*/
#include <pcl/surface/on_nurbs/fitting_surface_pdm.h>
#include <pcl/surface/on_nurbs/nurbs_data.h>
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
void
compute_quadfit ();
void
- compute_refinement (FittingSurface* fitting);
+ compute_refinement (FittingSurface* fitting) const;
void
- compute_boundary (FittingSurface* fitting);
+ compute_boundary (FittingSurface* fitting) const;
void
- compute_interior (FittingSurface* fitting);
+ compute_interior (FittingSurface* fitting) const;
Eigen::Vector2d
project (const Eigen::Vector3d &pt);
bool
- is_back_facing (const Eigen::Vector3d &v0,
- const Eigen::Vector3d &v1,
+ is_back_facing (const Eigen::Vector3d &v0,
+ const Eigen::Vector3d &v1,
const Eigen::Vector3d &v2,
const Eigen::Vector3d &v3);
* \param[in] intrinsic The camera projection matrix.
* \param[in] intrinsic The world matrix.*/
void
- setProjectionMatrix (const Eigen::Matrix4d &intrinsic,
+ setProjectionMatrix (const Eigen::Matrix4d &intrinsic,
const Eigen::Matrix4d &extrinsic);
/** \brief Compute point cloud and fit (multiple) models */
/** \brief Get error of each interior point (L2-norm of point to closest point on surface) and square-error */
void
- getInteriorError (std::vector<double> &error);
+ getInteriorError (std::vector<double> &error) const;
/** \brief Get error of each boundary point (L2-norm of point to closest point on surface) and square-error */
void
- getBoundaryError (std::vector<double> &error);
+ getBoundaryError (std::vector<double> &error) const;
/** \brief Get parameter of each interior point */
void
- getInteriorParams (std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ¶ms);
+ getInteriorParams (std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ¶ms) const;
/** \brief Get parameter of each boundary point */
void
- getBoundaryParams (std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ¶ms);
+ getBoundaryParams (std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ¶ms) const;
/** \brief get the normals to the interior points given by setInterior() */
void
- getInteriorNormals (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &normal);
+ getInteriorNormals (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &normal) const;
/** \brief get the normals to the boundary points given by setBoundary() */
void
- getBoundaryNormals (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &normals);
+ getBoundaryNormals (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &normals) const;
/** \brief Get the closest point on a NURBS from a point pt in parameter space
* \param[in] nurbs The NURBS surface
* \param[in] maxSteps Maximum iteration steps
* \param[in] accuracy Accuracy below which the iterations stop */
static void
- getClosestPointOnNurbs (ON_NurbsSurface nurbs,
- const Eigen::Vector3d &pt,
- Eigen::Vector2d& params,
+ getClosestPointOnNurbs (ON_NurbsSurface nurbs,
+ const Eigen::Vector3d &pt,
+ Eigen::Vector2d& params,
int maxSteps = 100,
double accuracy = 1e-4);
#pragma once
#include <pcl/common/angles.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/surface/reconstruction.h>
+
namespace pcl
{
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/surface/reconstruction.h>
#include <pcl/surface/boost.h>
#include <pcl/PolygonMesh.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/surface/reconstruction.h>
#include <pcl/common/transforms.h>
/** \brief Get the target reduction factor */
inline float
- getTargetReductionFactor ()
+ getTargetReductionFactor () const
{
return target_reduction_factor_;
}
/** \brief Get the number of iterations. */
inline int
- getNumIter ()
+ getNumIter () const
{
return num_iter_;
};
/** \brief Get the convergence criterion. */
inline float
- getConvergence ()
+ getConvergence () const
{
return convergence_;
};
/** \brief Get the relaxation factor of the Laplacian smoothing */
inline float
- getRelaxationFactor ()
+ getRelaxationFactor () const
{
return relaxation_factor_;
};
/** \brief Get the status of the feature edge smoothing */
inline bool
- getFeatureEdgeSmoothing ()
+ getFeatureEdgeSmoothing () const
{
return feature_edge_smoothing_;
};
/** \brief Get the angle threshold for considering an edge to be sharp */
inline float
- getFeatureAngle ()
+ getFeatureAngle () const
{
return feature_angle_;
};
/** \brief Get the edge angle to control smoothing along edges */
inline float
- getEdgeAngle ()
+ getEdgeAngle () const
{
return edge_angle_;
};
/** \brief Get the status of the boundary smoothing */
inline bool
- getBoundarySmoothing ()
+ getBoundarySmoothing () const
{
return boundary_smoothing_;
}
/** \brief Get the number of iterations. */
inline int
- getNumIter ()
+ getNumIter () const
{
return num_iter_;
};
/** \brief Get the pass band value. */
inline float
- getPassBand ()
+ getPassBand () const
{
return pass_band_;
};
/** \brief Get whether the coordinate normalization is active or not */
inline bool
- getNormalizeCoordinates ()
+ getNormalizeCoordinates () const
{
return normalize_coordinates_;
}
/** \brief Get the status of the feature edge smoothing */
inline bool
- getFeatureEdgeSmoothing ()
+ getFeatureEdgeSmoothing () const
{
return feature_edge_smoothing_;
};
/** \brief Get the angle threshold for considering an edge to be sharp */
inline float
- getFeatureAngle ()
+ getFeatureAngle () const
{
return feature_angle_;
};
/** \brief Get the edge angle to control smoothing along edges */
inline float
- getEdgeAngle ()
+ getEdgeAngle () const
{
return edge_angle_;
};
/** \brief Get the status of the boundary smoothing */
inline bool
- getBoundarySmoothing ()
+ getBoundarySmoothing () const
{
return boundary_smoothing_;
}
// SDKBREAK Oct 30, 07 - LW
// This function should not be used any longer
-PCL_DEPRECATED("Use the version that takes a model transform argument")
+PCL_DEPRECATED(1, 12, "Use the version that takes a model transform argument")
bool ON_Annotation2::GetTextXform(
ON_RECT /*gdi_text_rect*/,
const ON_Font& /*font*/,
// New function added Oct 30, 07 - LW
// To use model xform to draw annotation in blocks correctly
#if 0
-PCL_DEPRECATED("Use the version that takes a dimstyle pointer")
+PCL_DEPRECATED(1, 12, "Use the version that takes a dimstyle pointer")
bool ON_Annotation2::GetTextXform(
ON_RECT gdi_text_rect,
const ON_Font& font,
// SDKBREAK Oct 30, 07 - LW
// This function should not be used any longer
-PCL_DEPRECATED("Use the version that takes a model transform argument")
+PCL_DEPRECATED(1, 12, "Use the version that takes a model transform argument")
bool ON_Annotation2::GetTextXform(
ON_RECT /*gdi_text_rect*/,
int /*gdi_height_of_I*/,
{
unsigned char stack_buffer[4*64*sizeof(double)];
double delta_t;
- register double alpha0;
- register double alpha1;
- register double *cv0, *cv1;
- register int i, j, k;
+ double alpha0;
+ double alpha1;
+ double *cv0, *cv1;
+ int i, j, k;
double* CV, *tmp;
void* free_me = 0;
const int degree = order-1;
TL_EvNurbBasis
TL_EvNurbBasisDer
*****************************************************************************/
- register double a0, a1, x, y;
+ double a0, a1, x, y;
const double *k0;
double *t_k, *k_t, *N0;
const int d = order-1;
- register int j, r;
+ int j, r;
t_k = (double*)alloca( d<<4 );
k_t = t_k + d;
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2020-, OpenPerception
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <pcl/surface/3rdparty/poisson4/bspline_data.h>
+
+namespace pcl {
+namespace poisson {
+template <>
+void
+BSplineElements<1>::upSample(BSplineElements<1>& high) const
+{
+ high.resize(size() * 2);
+ high.assign(high.size(), BSplineElementCoefficients<1>());
+ for (int i = 0; i < int(size()); i++) {
+ high[2 * i + 0][0] += 1 * (*this)[i][0];
+ high[2 * i + 0][1] += 0 * (*this)[i][0];
+ high[2 * i + 1][0] += 2 * (*this)[i][0];
+ high[2 * i + 1][1] += 1 * (*this)[i][0];
+
+ high[2 * i + 0][0] += 1 * (*this)[i][1];
+ high[2 * i + 0][1] += 2 * (*this)[i][1];
+ high[2 * i + 1][0] += 0 * (*this)[i][1];
+ high[2 * i + 1][1] += 1 * (*this)[i][1];
+ }
+ high.denominator = denominator * 2;
+}
+
+template <>
+void
+BSplineElements<2>::upSample(BSplineElements<2>& high) const
+{
+ /* /----\
+ * / \
+ * / \ = 1 /--\ +3 /--\ +3 /--\ +1 /--\
+ * / \ / \ / \ / \ / \
+ * |----------| |----------| |----------| |----------| |----------|
+ */
+
+ high.resize(size() * 2);
+ high.assign(high.size(), BSplineElementCoefficients<2>());
+ for (int i = 0; i < int(size()); i++) {
+ high[2 * i + 0][0] += 1 * (*this)[i][0];
+ high[2 * i + 0][1] += 0 * (*this)[i][0];
+ high[2 * i + 0][2] += 0 * (*this)[i][0];
+ high[2 * i + 1][0] += 3 * (*this)[i][0];
+ high[2 * i + 1][1] += 1 * (*this)[i][0];
+ high[2 * i + 1][2] += 0 * (*this)[i][0];
+
+ high[2 * i + 0][0] += 3 * (*this)[i][1];
+ high[2 * i + 0][1] += 3 * (*this)[i][1];
+ high[2 * i + 0][2] += 1 * (*this)[i][1];
+ high[2 * i + 1][0] += 1 * (*this)[i][1];
+ high[2 * i + 1][1] += 3 * (*this)[i][1];
+ high[2 * i + 1][2] += 3 * (*this)[i][1];
+
+ high[2 * i + 0][0] += 0 * (*this)[i][2];
+ high[2 * i + 0][1] += 1 * (*this)[i][2];
+ high[2 * i + 0][2] += 3 * (*this)[i][2];
+ high[2 * i + 1][0] += 0 * (*this)[i][2];
+ high[2 * i + 1][1] += 0 * (*this)[i][2];
+ high[2 * i + 1][2] += 1 * (*this)[i][2];
+ }
+ high.denominator = denominator * 4;
+}
+} // namespace poisson
+} // namespace pcl
}
void
-SequentialFitter::compute_refinement (FittingSurface* fitting)
+SequentialFitter::compute_refinement (FittingSurface* fitting) const
{
// Refinement
FittingSurface::Parameter paramFP (0.0, 1.0, 0.0, m_params.forceBoundary, m_params.stiffnessBoundary, 0.0);
}
void
-SequentialFitter::compute_boundary (FittingSurface* fitting)
+SequentialFitter::compute_boundary (FittingSurface* fitting) const
{
FittingSurface::Parameter paramFP (0.0, 1.0, 0.0, m_params.forceBoundary, m_params.stiffnessBoundary, 0.0);
}
}
void
-SequentialFitter::compute_interior (FittingSurface* fitting)
+SequentialFitter::compute_interior (FittingSurface* fitting) const
{
// iterate interior points
//std::vector<double> wInt(m_data.interior.PointCount(), m_params.forceInterior);
}
void
-SequentialFitter::getInteriorError (std::vector<double> &error)
+SequentialFitter::getInteriorError (std::vector<double> &error) const
{
error = m_data.interior_error;
}
void
-SequentialFitter::getBoundaryError (std::vector<double> &error)
+SequentialFitter::getBoundaryError (std::vector<double> &error) const
{
error = m_data.boundary_error;
}
void
-SequentialFitter::getInteriorParams (std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ¶ms)
+SequentialFitter::getInteriorParams (std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ¶ms) const
{
params = m_data.interior_param;
}
void
-SequentialFitter::getBoundaryParams (std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ¶ms)
+SequentialFitter::getBoundaryParams (std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ¶ms) const
{
params = m_data.boundary_param;
}
void
-SequentialFitter::getInteriorNormals (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &normals)
+SequentialFitter::getInteriorNormals (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &normals) const
{
normals = m_data.interior_normals;
}
void
-SequentialFitter::getBoundaryNormals (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &normals)
+SequentialFitter::getBoundaryNormals (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &normals) const
{
normals = m_data.boundary_normals;
}
"${PCL_SOURCE_DIR}/test/2d/opening_binary.pcd"
"${PCL_SOURCE_DIR}/test/2d/closing_binary.pcd"
"${PCL_SOURCE_DIR}/test/2d/canny.pcd")
+
+PCL_ADD_TEST(test_2d_keypoint_instantiation_with_precompile test_2d_keypoint_instantiation_with_precompile FILES keypoint_instantiation.cpp LINK_WITH pcl_gtest pcl_common)
+
+PCL_ADD_TEST(test_2d_keypoint_instantiation_without_precompile test_2d_keypoint_instantiation_without_precompile FILES keypoint_instantiation.cpp LINK_WITH pcl_gtest pcl_common)
+target_compile_definitions(test_2d_keypoint_instantiation_without_precompile PUBLIC PCL_NO_PRECOMPILE)
--- /dev/null
+/**
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2020-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <pcl/2d/keypoint.h> // for pcl::Keypoint
+
+#include <pcl/point_types.h> // for pcl::PointXYZ
+#include <pcl/test/gtest.h> // for SUCCEED
+
+/** This isn't useful except for testing the instantiation of this class. See #3898 */
+TEST (Keypoint, instantiatesWithAndWithoutPrecompiledHeaders)
+{
+ pcl::Keypoint<pcl::PointXYZ> keypoint = pcl::Keypoint<pcl::PointXYZ>();
+ SUCCEED();
+}
+
+/** --[ */
+int
+main (int argc, char** argv)
+{
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+/* ]-- */
TEST (Convolution, borderOptions)
{
- kernel<pcl::PointXYZI> *k = new kernel<pcl::PointXYZI> ();
- Convolution<pcl::PointXYZI> *conv = new Convolution<pcl::PointXYZI> ();
+ kernel<pcl::PointXYZI> k;
+ Convolution<pcl::PointXYZI> conv;
/*dummy clouds*/
pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
int height = input_cloud->height;
int width = input_cloud->width;
- k
->setKernelType(kernel<pcl::PointXYZI>::DERIVATIVE_CENTRAL_X);
- k->fetchKernel (*kernel_cloud);
+ k
.setKernelType(kernel<pcl::PointXYZI>::DERIVATIVE_CENTRAL_X);
+ k.fetchKernel (*kernel_cloud);
- conv->setKernel(*kernel_cloud);
- conv->setInputCloud(input_cloud);
+ conv.setKernel(*kernel_cloud);
+ conv.setInputCloud(input_cloud);
- conv
->setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_MIRROR);
- conv->filter (*output_cloud);
+ conv
.setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_MIRROR);
+ conv.filter (*output_cloud);
for (int i = 1; i < height - 1; i++)
for (int j = 1; j < width - 1; j++)
EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1);
EXPECT_NEAR ((*output_cloud)(width-1,i).intensity, ((*input_cloud)(width-1,i).intensity-(*input_cloud)(width-2,i).intensity), 1);
}
- conv
->setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_CLAMP);
- conv->filter (*output_cloud);
+ conv
.setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_CLAMP);
+ conv.filter (*output_cloud);
for (int i = 1; i < height - 1; i++)
for (int j = 1; j < width - 1; j++)
EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1);
// EXPECT_NEAR ((*output_cloud)(width-1,i).intensity, ((*input_cloud)(width-1,i).intensity-(*input_cloud)(width-2,i).intensity), 1);
}
- conv
->setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_ZERO_PADDING);
- conv->filter (*output_cloud);
+ conv
.setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_ZERO_PADDING);
+ conv.filter (*output_cloud);
for (int i = 1; i < height - 1; i++)
for (int j = 1; j < width - 1; j++)
EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1);
TEST (Convolution, gaussianSmooth)
{
- kernel<pcl::PointXYZI> *k = new kernel<pcl::PointXYZI> ();
- Convolution<pcl::PointXYZI> *conv = new Convolution<pcl::PointXYZI> ();
+ kernel<pcl::PointXYZI> k;
+ Convolution<pcl::PointXYZI> conv;
/*dummy clouds*/
pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
int height = input_cloud->height;
int width = input_cloud->width;
- k
->setKernelType(kernel<pcl::PointXYZI>::GAUSSIAN);
- k->setKernelSize(3);
- k->setKernelSigma(1.0f);
- k->fetchKernel (*kernel_cloud);
+ k
.setKernelType(kernel<pcl::PointXYZI>::GAUSSIAN);
+ k.setKernelSize(3);
+ k.setKernelSigma(1.0f);
+ k.fetchKernel (*kernel_cloud);
- conv->setKernel(*kernel_cloud);
- conv->setInputCloud(input_cloud);
+ conv.setKernel(*kernel_cloud);
+ conv.setInputCloud(input_cloud);
- conv
->setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_MIRROR);
- conv->filter (*output_cloud);
+ conv
.setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_MIRROR);
+ conv.filter (*output_cloud);
for (int i = 1; i < height - 1; i++)
for (int j = 1; j < width - 1; j++)
EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity, 1);
-
}
TEST(Edge, sobel)
pcl::io::loadPCDFile(lena, *input_cloud);
pcl::io::loadPCDFile(erosion_ref, *gt_output_cloud);
- Morphology<pcl::PointXYZI> *morph = new Morphology<pcl::PointXYZI>();
- morph->setInputCloud(input_cloud);
- morph->structuringElementRectangle(*structuring_element_cloud, 3, 3);
- morph->setStructuringElement(structuring_element_cloud);
- morph->erosionGray(*output_cloud);
+ Morphology<pcl::PointXYZI> morph;
+ morph.setInputCloud(input_cloud);
+ morph.structuringElementRectangle(*structuring_element_cloud, 3, 3);
+ morph.setStructuringElement(structuring_element_cloud);
+ morph.erosionGray(*output_cloud);
int height = input_cloud->height;
int width = input_cloud->width;
pcl::io::loadPCDFile(erosion_binary_ref, *gt_output_cloud);
threshold(input_cloud, 100);
- morph->setInputCloud(input_cloud);
- morph->erosionBinary(*output_cloud);
+ morph.setInputCloud(input_cloud);
+ morph.erosionBinary(*output_cloud);
for (int i = 1; i < height - 1; i++)
for (int j = 1; j < width - 1; j++)
EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1);
-
}
TEST(Morphology, dilation)
pcl::io::loadPCDFile(lena, *input_cloud);
pcl::io::loadPCDFile(dilation_ref, *gt_output_cloud);
- Morphology<pcl::PointXYZI> *morph = new Morphology<pcl::PointXYZI>();
- morph->setInputCloud(input_cloud);
- morph->structuringElementRectangle(*structuring_element_cloud, 3, 3);
- morph->setStructuringElement(structuring_element_cloud);
- morph->dilationGray(*output_cloud);
+ Morphology<pcl::PointXYZI> morph;
+ morph.setInputCloud(input_cloud);
+ morph.structuringElementRectangle(*structuring_element_cloud, 3, 3);
+ morph.setStructuringElement(structuring_element_cloud);
+ morph.dilationGray(*output_cloud);
int height = input_cloud->height;
int width = input_cloud->width;
pcl::io::loadPCDFile(dilation_binary_ref, *gt_output_cloud);
threshold(input_cloud, 100);
- morph->setInputCloud(input_cloud);
- morph->dilationBinary(*output_cloud);
+ morph.setInputCloud(input_cloud);
+ morph.dilationBinary(*output_cloud);
for (int i = 1; i < height - 1; i++)
for (int j = 1; j < width - 1; j++)
EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1);
pcl::io::loadPCDFile(lena, *input_cloud);
pcl::io::loadPCDFile(opening_ref, *gt_output_cloud);
- Morphology<pcl::PointXYZI> *morph = new Morphology<pcl::PointXYZI>();
- morph->setInputCloud(input_cloud);
- morph->structuringElementRectangle(*structuring_element_cloud, 3, 3);
- morph->setStructuringElement(structuring_element_cloud);
- morph->openingGray(*output_cloud);
+ Morphology<pcl::PointXYZI> morph;
+ morph.setInputCloud(input_cloud);
+ morph.structuringElementRectangle(*structuring_element_cloud, 3, 3);
+ morph.setStructuringElement(structuring_element_cloud);
+ morph.openingGray(*output_cloud);
int height = input_cloud->height;
int width = input_cloud->width;
pcl::io::loadPCDFile(opening_binary_ref, *gt_output_cloud);
threshold(input_cloud, 100);
- morph->setInputCloud(input_cloud);
- morph->openingBinary(*output_cloud);
+ morph.setInputCloud(input_cloud);
+ morph.openingBinary(*output_cloud);
for (int i = 1; i < height - 1; i++)
for (int j = 1; j < width - 1; j++)
EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1);
pcl::io::loadPCDFile(lena, *input_cloud);
pcl::io::loadPCDFile(closing_ref, *gt_output_cloud);
- Morphology<pcl::PointXYZI> *morph = new Morphology<pcl::PointXYZI>();
- morph->setInputCloud(input_cloud);
- morph->structuringElementRectangle(*structuring_element_cloud, 3, 3);
- morph->setStructuringElement(structuring_element_cloud);
- morph->closingGray(*output_cloud);
+ Morphology<pcl::PointXYZI> morph;
+ morph.setInputCloud(input_cloud);
+ morph.structuringElementRectangle(*structuring_element_cloud, 3, 3);
+ morph.setStructuringElement(structuring_element_cloud);
+ morph.closingGray(*output_cloud);
int height = input_cloud->height;
int width = input_cloud->width;
pcl::io::loadPCDFile(closing_binary_ref, *gt_output_cloud);
threshold(input_cloud, 100);
- morph->setInputCloud(input_cloud);
- morph->closingBinary(*output_cloud);
+ morph.setInputCloud(input_cloud);
+ morph.closingBinary(*output_cloud);
for (int i = 1; i < height - 1; i++)
for (int j = 1; j < width - 1; j++)
EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1);
set(DEFAULT OFF)
set(build TRUE)
+set(REASON "Disabled by default")
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS ${OPT_DEPS})
add_custom_target(tests "${CMAKE_CTEST_COMMAND}" -C $<$<CONFIG:Debug>:Debug>$<$<CONFIG:Release>:Release> -V -T Test VERBATIM)
else()
add_custom_target(tests "${CMAKE_CTEST_COMMAND}" -V -T Test VERBATIM)
-endif()
+endif()
set_target_properties(tests PROPERTIES FOLDER "Tests")
#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
-#include <pcl/common/point_operators.h>
#include <pcl/common/intensity.h>
using namespace pcl;
}
///////////////////////////////////////////////////////////////////////////////////////////
+// Ignore unknown pragma warning on MSVC (4996)
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable: 4068)
+#endif
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#pragma GCC diagnostic push
TEST (PCL, concatenatePointCloud)
}
}
#pragma GCC diagnostic pop
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
///////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, concatenatePointCloud2)
#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
-#include <pcl/common/point_operators.h>
using namespace pcl;
using namespace pcl::test;
#include <pcl/test/gtest.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
*/
-#include <pcl/point_traits.h>
+#include <pcl/memory.h> // for pcl::has_custom_allocator, PCL_MAKE_ALIGNED_OPERATOR_NEW
+#include <pcl/type_traits.h>
#include <pcl/point_types.h>
+#include <pcl/common/point_tests.h> // for pcl::isXYFinite, pcl::isXYZFinite, pcl::isNormalFinite
#include <pcl/test/gtest.h>
struct Foo
{
public:
+ // Manually ignore warnings here because of an issue in Eigen which
+ // results in a local typedef being unused inside the new and delete
+ // operators added by Eigen for C++14 or lower standards
+ /** \todo Remove for C++17 (or future standards)
+ */
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wunused-local-typedef"
PCL_MAKE_ALIGNED_OPERATOR_NEW
+ #pragma clang diagnostic pop
};
struct Bar
pcl::IndicesPtr indicesptr (new pcl::Indices ());
indicesptr->resize(cloudptr->size() / 2);
- for (int i = 0; i < cloudptr->size() / 2; ++i)
+ for (std::size_t i = 0; i < cloudptr->size() / 2; ++i)
{
(*indicesptr)[i] = i + cloudptr->size() / 2;
}
std::vector<PointXYZ> normalsVec;
normalsVec.resize(normals->size());
- for( int i = 0; i < normals->size(); ++i )
+ for(std::size_t i = 0; i < normals->size(); ++i )
{
normalsVec[i].x = normals->points[i].normal_x;
normalsVec[i].y = normals->points[i].normal_y;
*/
#include <pcl/test/gtest.h>
+#include <pcl/memory.h> // for pcl::make_shared
+#include <pcl/pcl_base.h> // for pcl::Indices
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
#include <pcl/filters/box_clipper3D.h>
input->push_back (PointXYZ (-0.9f, -0.9f, -0.9f));
ExtractIndices<PointXYZ> extract_indices;
- std::vector<int> indices;
+ pcl::Indices indices;
BoxClipper3D<PointXYZ> boxClipper3D (Affine3f::Identity ());
boxClipper3D.clipPointCloud3D (*input, indices);
PointCloud<PointXYZ> cloud_out;
extract_indices.setInputCloud (input);
- extract_indices.setIndices (boost::make_shared<vector<int> > (indices));
+ extract_indices.setIndices (pcl::make_shared<pcl::Indices> (indices));
extract_indices.filter (cloud_out);
EXPECT_EQ (int (indices.size ()), 9);
ps.setFilterLimits (0.99, 2.01);
for (int i = 0; i < 2; i++)
{
- ps.setFilterLimitsNegative ((bool)i);
+ ps.setNegative ((bool)i);
ps.filter (scf);
std::cerr << scf.points.size () << std::endl;
for (std::size_t j = 0; j < scf.points.size (); ++j)
EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
- pt.setFilterLimitsNegative (true);
+ pt.setNegative (true);
pt.filter (output);
EXPECT_EQ (int (output.points.size ()), 355);
EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
- pt_.setFilterLimitsNegative (true);
+ pt_.setNegative (true);
pt_.filter (output);
EXPECT_EQ (int (output.points.size ()), 355);
EXPECT_NEAR (output.points[output.points.size () - 1].x, cloud->points[cloud->points.size () - 1].x, 1e-5);
pt.setFilterFieldName ("z");
- pt.setFilterLimitsNegative (false);
+ pt.setNegative (false);
pt.setKeepOrganized (true);
pt.filter (output);
EXPECT_TRUE (std::isnan (output.points[41].y));
EXPECT_TRUE (std::isnan (output.points[41].z));
- pt.setFilterLimitsNegative (true);
+ pt.setNegative (true);
pt.filter (output);
EXPECT_EQ (output.points.size (), cloud->points.size ());
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (NormalSpaceSampling, Filters)
{
+ // pcl::Normal is not precompiled by default so we use PointNormal
+ auto cloud = pcl::make_shared<PointCloud<PointNormal>> ();
+ // generate 16 points (8 unique) with unit norm
+ cloud->reserve (16);
+ // ensure that the normals have unit norm
+ const auto value = std::sqrt(1/3.f);
+ for (int unique = 0; unique < 8; ++unique) {
+ const auto i = ((unique % 2) < 1) ? -1 : 1; // points alternate sign
+ const auto j = ((unique % 4) < 2) ? -1 : 1; // 2 points negative, 2 positive
+ const auto k = ((unique % 8) < 4) ? -1 : 1; // first 4 points negative, rest positive
+ for (int duplicate = 0; duplicate < 2; ++duplicate) {
+ cloud->emplace_back (0.f, 0.f, 0.f, i * value, j * value, k * value);
+ }
+ }
+
NormalSpaceSampling<PointNormal, PointNormal> normal_space_sampling;
- normal_space_sampling.setInputCloud (cloud_walls_normals);
- normal_space_sampling.setNormals (cloud_walls_normals);
- normal_space_sampling.setBins (4, 4, 4);
+ normal_space_sampling.setInputCloud (cloud);
+ normal_space_sampling.setNormals (cloud);
+ normal_space_sampling.setBins (2, 2, 2);
normal_space_sampling.setSeed (0);
- normal_space_sampling.setSample (static_cast<unsigned int> (cloud_walls_normals->size ()) / 4);
+ normal_space_sampling.setSample (8);
- IndicesPtr walls_indices (new std::vector<int> ());
+ IndicesPtr walls_indices = pcl::make_shared<Indices> ();
normal_space_sampling.filter (*walls_indices);
- CovarianceSampling<PointNormal, PointNormal> covariance_sampling;
- covariance_sampling.setInputCloud (cloud_walls_normals);
- covariance_sampling.setNormals (cloud_walls_normals);
- covariance_sampling.setIndices (walls_indices);
- covariance_sampling.setNumberOfSamples (0);
- double cond_num_walls_sampled = covariance_sampling.computeConditionNumber ();
-
+ // The orientation space of the normals is divided into 2x2x2 buckets
+ // points are samples arbitrarily from each bucket in succession until the
+ // requested number of samples is met. This means we expect to see only one index
+ // for every two elements in the original array e.g. 0, 3, 4, 6, etc...
+ // if 0 is sampled, index 1 can no longer be there and so forth
+ std::array<std::set<index_t>, 8> buckets;
+ for (const auto index : *walls_indices)
+ buckets[index/2].insert (index);
- EXPECT_NEAR (33.04893, cond_num_walls_sampled, 1e-1);
- EXPECT_EQ (1412, (*walls_indices)[0]);
- EXPECT_EQ (1943, (*walls_indices)[walls_indices->size () / 4]);
- EXPECT_EQ (2771, (*walls_indices)[walls_indices->size () / 2]);
- EXPECT_EQ (3215, (*walls_indices)[walls_indices->size () * 3 / 4]);
- EXPECT_EQ (2503, (*walls_indices)[walls_indices->size () - 1]);
+ EXPECT_EQ (8u, walls_indices->size ());
+ for (const auto& bucket : buckets)
+ EXPECT_EQ (1u, bucket.size ());
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include <pcl/test/gtest.h>
#include <pcl/geometry/triangle_mesh.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include "test_mesh_common_functions.h"
#include <pcl/geometry/quad_mesh.h>
#include <pcl/geometry/polygon_mesh.h>
#include <pcl/geometry/mesh_conversion.h>
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/test/gtest.h>
#include <pcl/PCLPointCloud2.h>
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/console/print.h>
for (int compression_profile = pcl::io::LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
compression_profile != pcl::io::COMPRESSION_PROFILE_COUNT; ++compression_profile) {
// instantiate point cloud compression encoder/decoder
- pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* pointcloud_encoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>((pcl::io::compression_Profiles_e) compression_profile, false);
- pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* pointcloud_decoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>();
+ pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> pointcloud_encoder((pcl::io::compression_Profiles_e) compression_profile, false);
+ pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> pointcloud_decoder;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGBA>());
// iterate over runs
for (int test_idx = 0; test_idx < NUMBER_OF_TEST_RUNS; test_idx++, total_runs++)
// std::cout << "Run: " << total_runs << " compression profile:" << compression_profile << " point_count: " << point_count;
std::stringstream compressed_data;
- pointcloud_encoder->encodePointCloud(cloud, compressed_data);
- pointcloud_decoder->decodePointCloud(compressed_data, cloud_out);
+ pointcloud_encoder.encodePointCloud(cloud, compressed_data);
+ pointcloud_decoder.decodePointCloud(compressed_data, cloud_out);
EXPECT_GT((int)cloud_out->width, 0) << "decoded PointCloud width <= 0";
EXPECT_GT((int)cloud_out->height, 0) << " decoded PointCloud height <= 0 ";
}
compression_profile != pcl::io::COMPRESSION_PROFILE_COUNT; ++compression_profile)
{
// instantiate point cloud compression encoder/decoder
- pcl::io::OctreePointCloudCompression<pcl::PointXYZ>* pointcloud_encoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>((pcl::io::compression_Profiles_e) compression_profile, false);
- pcl::io::OctreePointCloudCompression<pcl::PointXYZ>* pointcloud_decoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>();
+ pcl::io::OctreePointCloudCompression<pcl::PointXYZ> pointcloud_encoder((pcl::io::compression_Profiles_e) compression_profile, false);
+ pcl::io::OctreePointCloudCompression<pcl::PointXYZ> pointcloud_decoder;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>());
// loop over runs
for (int test_idx = 0; test_idx < NUMBER_OF_TEST_RUNS; test_idx++, total_runs++)
std::stringstream compressed_data;
try
{ // decodePointCloud() throws exceptions on errors
- pointcloud_encoder->encodePointCloud(cloud, compressed_data);
- pointcloud_decoder->decodePointCloud(compressed_data, cloud_out);
+ pointcloud_encoder.encodePointCloud(cloud, compressed_data);
+ pointcloud_decoder.decodePointCloud(compressed_data, cloud_out);
EXPECT_GT((int)cloud_out->width, 0) << "decoded PointCloud width <= 0";
EXPECT_GT((int)cloud_out->height, 0) << " decoded PointCloud height <= 0 ";
}
#include <pcl/test/gtest.h>
#include <pcl/PCLPointCloud2.h>
-#include <pcl/point_traits.h>
+#include <pcl/type_traits.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/console/print.h>
*
*/
-#include <pcl/test/gtest.h>
-#include <iostream> // For debug
-#include <map>
-#include <pcl/common/time.h>
+#include <pcl/kdtree/impl/kdtree_flann.hpp>
+
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/distances.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+#include <pcl/common/time.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/kdtree/impl/kdtree_flann.hpp>
+#include <pcl/test/gtest.h>
+
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
+#include <iostream> // For debug
+#include <map>
+
using namespace std;
using namespace pcl;
*/
}
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// This test if the ICP algorithm can successfully find the transformation of a cloud that has been
+// moved 0.7 in x direction.
+// It indirectly test the KDTree doesn't get an empty input cloud, see #3624
+// It is more or less a copy of https://github.com/PointCloudLibrary/pcl/blob/cc7fe363c6463a0abc617b1e17e94ab4bd4169ef/doc/tutorials/content/sources/iterative_closest_point/iterative_closest_point.cpp
+TEST(PCL, ICP_translated)
+{
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(5,1));
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
+
+ // Fill in the CloudIn data
+ for (auto& point : *cloud_in)
+ {
+ point.x = 1024 * rand() / (RAND_MAX + 1.0f);
+ point.y = 1024 * rand() / (RAND_MAX + 1.0f);
+ point.z = 1024 * rand() / (RAND_MAX + 1.0f);
+ }
+
+ *cloud_out = *cloud_in;
+
+ for (auto& point : *cloud_out)
+ point.x += 0.7f;
+
+ pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
+ icp.setInputSource(cloud_in);
+ icp.setInputTarget(cloud_out);
+
+ pcl::PointCloud<pcl::PointXYZ> Final;
+ icp.align(Final);
+
+ // Check that we have sucessfully converged
+ ASSERT_EQ(icp.hasConverged(), true);
+
+ // Test that the fitness score is below acceptable threshold
+ EXPECT_LT(icp.getFitnessScore(), 1e-6);
+
+ // Ensure that the translation found is within acceptable threshold.
+ EXPECT_NEAR(icp.getFinalTransformation()(0, 3), 0.7, 2e-3);
+ EXPECT_NEAR(icp.getFinalTransformation()(1, 3), 0.0, 2e-3);
+ EXPECT_NEAR(icp.getFinalTransformation()(2, 3), 0.0, 2e-3);
+}
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, IterativeClosestPoint)
{
reg.align (cloud_reg);
EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ()));
- //Eigen::Matrix4f transformation = reg.getFinalTransformation ();
-// EXPECT_NEAR (transformation (0, 0), 0.8806, 1e-3);
-// EXPECT_NEAR (transformation (0, 1), 0.036481287330389023, 1e-2);
-// EXPECT_NEAR (transformation (0, 2), -0.4724, 1e-3);
-// EXPECT_NEAR (transformation (0, 3), 0.03453, 1e-3);
-//
-// EXPECT_NEAR (transformation (1, 0), -0.02354, 1e-3);
-// EXPECT_NEAR (transformation (1, 1), 0.9992, 1e-3);
-// EXPECT_NEAR (transformation (1, 2), 0.03326, 1e-3);
-// EXPECT_NEAR (transformation (1, 3), -0.001519, 1e-3);
-//
-// EXPECT_NEAR (transformation (2, 0), 0.4732, 1e-3);
-// EXPECT_NEAR (transformation (2, 1), -0.01817, 1e-3);
-// EXPECT_NEAR (transformation (2, 2), 0.8808, 1e-3);
-// EXPECT_NEAR (transformation (2, 3), 0.04116, 1e-3);
-//
-// EXPECT_EQ (transformation (3, 0), 0);
-// EXPECT_EQ (transformation (3, 1), 0);
-// EXPECT_EQ (transformation (3, 2), 0);
-// EXPECT_EQ (transformation (3, 3), 1);
+ Eigen::Matrix4f transformation = reg.getFinalTransformation ();
+ EXPECT_NEAR (transformation (0, 0), 0.8806, 1e-3);
+ EXPECT_NEAR (transformation (0, 1), 0.036481287330389023, 1e-2);
+ EXPECT_NEAR (transformation (0, 2), -0.4724, 1e-3);
+ EXPECT_NEAR (transformation (0, 3), 0.03453, 1e-3);
+
+ EXPECT_NEAR (transformation (1, 0), -0.02354, 1e-3);
+ EXPECT_NEAR (transformation (1, 1), 0.9992, 1e-3);
+ EXPECT_NEAR (transformation (1, 2), 0.03326, 1e-3);
+ EXPECT_NEAR (transformation (1, 3), -0.001519, 1e-3);
+
+ EXPECT_NEAR (transformation (2, 0), 0.4732, 1e-3);
+ EXPECT_NEAR (transformation (2, 1), -0.01817, 1e-3);
+ EXPECT_NEAR (transformation (2, 2), 0.8808, 1e-3);
+ EXPECT_NEAR (transformation (2, 3), 0.04116, 1e-3);
+
+ EXPECT_EQ (transformation (3, 0), 0);
+ EXPECT_EQ (transformation (3, 1), 0);
+ EXPECT_EQ (transformation (3, 2), 0);
+ EXPECT_EQ (transformation (3, 3), 1);
}
TEST (PCL, IterativeClosestPointWithNormals)
std::priority_queue<prioPointQueueEntry, std::vector<prioPointQueueEntry, Eigen::aligned_allocator<prioPointQueueEntry> > > pointCandidates;
// create octree
- pcl::search::Search<PointXYZ>* octree = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::OCTREE);
+ pcl::search::Search<PointXYZ> octree(pcl::search::OCTREE);
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
}
// octree nearest neighbor search
- octree->setInputCloud (cloudIn);
- octree->nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances);
+ octree.setInputCloud (cloudIn);
+ octree.nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances);
ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
TEST (PCL, KdTreeWrapper_nearestKSearch)
{
- pcl::search::Search<PointXYZ>* kdtree = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::KDTREE);
- kdtree->setInputCloud (cloud.makeShared ());
+ pcl::search::Search<PointXYZ> kdtree(pcl::search::KDTREE);
+ kdtree.setInputCloud (cloud.makeShared ());
PointXYZ test_point (0.01f, 0.01f, 0.01f);
unsigned int no_of_neighbors = 20;
multimap<float, int> sorted_brute_force_result;
std::vector<float> k_distances;
k_distances.resize (no_of_neighbors);
- kdtree->nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
+ kdtree.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
EXPECT_EQ (k_indices.size (), no_of_neighbors);
ScopeTime scopeTime ("FLANN nearestKSearch");
{
- pcl::search::Search<PointXYZ>* kdtree = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::KDTREE);
- //kdtree->initSearchDS ();
- kdtree->setInputCloud (cloud_big.makeShared ());
+ pcl::search::AutotunedSearch<PointXYZ> kdtree (pcl::search::KDTREE);
+ //kdtree.initSearchDS ();
+ kdtree.setInputCloud (cloud_big.makeShared ());
for (std::size_t i = 0; i < cloud_big.points.size (); ++i)
- kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
+ kdtree.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
}
-
}
/* Function to auto evaluate the best search structure for the given dataset */
TEST (PCL, AutoTunedSearch_Evaluate)
{
- pcl::search::Search<PointXYZ>* search = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::AUTO_TUNED);
+ pcl::search::AutotunedSearch<PointXYZ> search (pcl::search::AUTO_TUNED);
pcl::PCDReader pcd;
pcl::PointCloud<PointXYZ>::Ptr cloudIn (new pcl::PointCloud<PointXYZ>);
return;
}
- search->evaluateSearchMethods (cloudIn, pcl::search::NEAREST_K_SEARCH);
- search->evaluateSearchMethods (cloudIn, pcl::search::NEAREST_RADIUS_SEARCH);
+ search.evaluateSearchMethods (cloudIn, pcl::search::NEAREST_K_SEARCH);
+ search.evaluateSearchMethods (cloudIn, pcl::search::NEAREST_RADIUS_SEARCH);
}
init ();
// Testing using explicit instantiation of inherited class
- pcl::search::Search<PointXYZ>* kdtree = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::KDTREE);
- kdtree->setInputCloud (cloud.makeShared ());
+ pcl::search::AutotunedSearch<PointXYZ> kdtree(pcl::search::KDTREE);
+ kdtree.setInputCloud (cloud.makeShared ());
return (RUN_ALL_TESTS ());
};
/* Test for FlannSearch nearestKSearch */
TEST (PCL, FlannSearch_nearestKSearch)
{
- pcl::search::
Search<PointXYZ>* FlannSearch = new pcl::search::FlannSearch<PointXYZ> (new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
- FlannSearch->setInputCloud (cloud.makeShared ());
+ pcl::search::
FlannSearch<PointXYZ> FlannSearch (new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
+ FlannSearch.setInputCloud (cloud.makeShared ());
PointXYZ test_point (0.01f, 0.01f, 0.01f);
unsigned int no_of_neighbors = 20;
multimap<float, int> sorted_brute_force_result;
std::vector<float> k_distances;
k_distances.resize (no_of_neighbors);
- FlannSearch->nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
+ FlannSearch.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
//if (k_indices.size () != no_of_neighbors) std::cerr << "Found "<<k_indices.size ()<<" instead of "<<no_of_neighbors<<" neighbors.\n";
EXPECT_EQ (k_indices.size (), no_of_neighbors);
ScopeTime scopeTime ("FLANN nearestKSearch");
{
- pcl::search::
Search<PointXYZ>* FlannSearch = new pcl::search::FlannSearch<PointXYZ>( new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
- //FlannSearch->initSearchDS ();
- FlannSearch->setInputCloud (cloud_big.makeShared ());
+ pcl::search::
FlannSearch<PointXYZ> FlannSearch( new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
+ //FlannSearch.initSearchDS ();
+ FlannSearch.setInputCloud (cloud_big.makeShared ());
for (const auto &point : cloud_big.points)
- FlannSearch->nearestKSearch (point, no_of_neighbors, k_indices, k_distances);
+ FlannSearch.nearestKSearch (point, no_of_neighbors, k_indices, k_distances);
}
}
unsigned int no_of_neighbors = 20;
- pcl::search::
Search<PointXYZ>* FlannSearch = new pcl::search::FlannSearch<PointXYZ> (new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
- //FlannSearch->initSearchDS ();
- FlannSearch->setInputCloud (cloud_big.makeShared ());
+ pcl::search::
FlannSearch<PointXYZ> FlannSearch (new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
+ //FlannSearch.initSearchDS ();
+ FlannSearch.setInputCloud (cloud_big.makeShared ());
PointCloud<PointXYZRGB> cloud_rgb;
std::vector< std::vector< float > > dists;
std::vector< std::vector< int > > indices;
- FlannSearch->nearestKSearchT (cloud_rgb, std::vector<int> (),no_of_neighbors,indices,dists);
+ FlannSearch.nearestKSearchT (cloud_rgb, std::vector<int> (),no_of_neighbors,indices,dists);
std::vector<int> k_indices;
k_indices.resize (no_of_neighbors);
for (std::size_t i = 0; i < cloud_rgb.points.size (); ++i)
{
- //FlannSearch->nearestKSearchT (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t);
- FlannSearch->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
+ //FlannSearch.nearestKSearchT (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t);
+ FlannSearch.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
EXPECT_EQ (k_indices.size (), indices[i].size ());
EXPECT_EQ (k_distances.size (), dists[i].size ());
for (std::size_t j = 0; j< no_of_neighbors; j++)
unsigned int no_of_neighbors = 20;
- pcl::search::
Search<PointXYZ>* FlannSearch = new pcl::search::FlannSearch<PointXYZ> (new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
- //FlannSearch->initSearchDS ();
- FlannSearch->setInputCloud (cloud_big.makeShared ());
+ pcl::search::
FlannSearch<PointXYZ> FlannSearch (new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
+ //FlannSearch.initSearchDS ();
+ FlannSearch.setInputCloud (cloud_big.makeShared ());
std::vector< std::vector< float > > dists;
std::vector< std::vector< int > > indices;
- FlannSearch->nearestKSearch (cloud_big, std::vector<int>(),no_of_neighbors,indices,dists);
+ FlannSearch.nearestKSearch (cloud_big, std::vector<int>(),no_of_neighbors,indices,dists);
std::vector<int> k_indices;
k_indices.resize (no_of_neighbors);
for (std::size_t i = 0; i < cloud_big.points.size (); ++i)
{
- FlannSearch->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
+ FlannSearch.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
EXPECT_EQ (k_indices.size (), indices[i].size ());
EXPECT_EQ (k_distances.size (), dists[i].size ());
for (std::size_t j = 0; j< no_of_neighbors; j++ )
unsigned int no_of_neighbors = 3;
- pcl::search::
Search<PointXYZ>* flann_search = new pcl::search::FlannSearch<PointXYZ> (new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
+ pcl::search::
FlannSearch<PointXYZ> flann_search (new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
//FlannSearch->initSearchDS ();
- flann_search->setInputCloud (cloud_big.makeShared ());
+ flann_search.setInputCloud (cloud_big.makeShared ());
std::vector< std::vector< float > > dists;
std::vector< std::vector< int > > indices;
{
query_indices.push_back (int (i));
}
- flann_search->nearestKSearch (cloud_big, query_indices,no_of_neighbors,indices,dists);
+ flann_search.nearestKSearch (cloud_big, query_indices,no_of_neighbors,indices,dists);
std::vector<int> k_indices;
k_indices.resize (no_of_neighbors);
for (std::size_t i = 0; i < query_indices.size (); ++i)
{
- flann_search->nearestKSearch (cloud_big[2*i], no_of_neighbors, k_indices, k_distances);
+ flann_search.nearestKSearch (cloud_big[2*i], no_of_neighbors, k_indices, k_distances);
EXPECT_EQ (k_indices.size (), indices[i].size ());
EXPECT_EQ (k_distances.size (), dists[i].size ());
for (std::size_t j = 0; j< no_of_neighbors; j++)
{
EXPECT_TRUE (k_indices[j] == indices[i][j] || k_distances[j] == dists[i][j]);
}
- flann_search->nearestKSearch (cloud_big,query_indices[i], no_of_neighbors, k_indices, k_distances);
+ flann_search.nearestKSearch (cloud_big,query_indices[i], no_of_neighbors, k_indices, k_distances);
EXPECT_EQ (k_indices.size (), indices[i].size ());
EXPECT_EQ (k_distances.size (), dists[i].size ());
for (std::size_t j = 0; j< no_of_neighbors; j++)
init ();
// Testing using explicit instantiation of inherited class
- pcl::search::
Search<PointXYZ>* FlannSearch = new pcl::search::FlannSearch<PointXYZ> ( new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
- FlannSearch->setInputCloud (cloud.makeShared ());
+ pcl::search::
FlannSearch<PointXYZ> FlannSearch ( new search::FlannSearch<PointXYZ>::KdTreeIndexCreator);
+ FlannSearch.setInputCloud (cloud.makeShared ());
return (RUN_ALL_TESTS ());
}
/* Test for KdTree nearestKSearch */TEST (PCL, KdTree_nearestKSearch)
{
- pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ> ();
- kdtree->setInputCloud (cloud.makeShared ());
+ pcl::search::KdTree<PointXYZ> kdtree;
+ kdtree.setInputCloud (cloud.makeShared ());
PointXYZ test_point (0.01f, 0.01f, 0.01f);
unsigned int no_of_neighbors = 20;
multimap<float, int> sorted_brute_force_result;
std::vector<float> k_distances;
k_distances.resize (no_of_neighbors);
- kdtree->nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
+ kdtree.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
//if (k_indices.size () != no_of_neighbors) std::cerr << "Found "<<k_indices.size ()<<" instead of "<<no_of_neighbors<<" neighbors.\n";
EXPECT_EQ (k_indices.size (), no_of_neighbors);
ScopeTime scopeTime ("FLANN nearestKSearch");
{
- pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ>();
- //kdtree->initSearchDS ();
- kdtree->setInputCloud (cloud_big.makeShared ());
+ pcl::search::KdTree<PointXYZ> kdtree;
+ //kdtree.initSearchDS ();
+ kdtree.setInputCloud (cloud_big.makeShared ());
for (const auto &point : cloud_big.points)
- kdtree->nearestKSearch (point, no_of_neighbors, k_indices, k_distances);
+ kdtree.nearestKSearch (point, no_of_neighbors, k_indices, k_distances);
}
}
{
unsigned int no_of_neighbors = 20;
- pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ> ();
- //kdtree->initSearchDS ();
- kdtree->setInputCloud (cloud_big.makeShared ());
+ pcl::search::KdTree<PointXYZ> kdtree;
+ //kdtree.initSearchDS ();
+ kdtree.setInputCloud (cloud_big.makeShared ());
PointCloud<PointXYZRGB> cloud_rgb;
std::vector< std::vector< float > > dists;
std::vector< std::vector< int > > indices;
- kdtree->nearestKSearchT (cloud_rgb, std::vector<int> (),no_of_neighbors,indices,dists);
+ kdtree.nearestKSearchT (cloud_rgb, std::vector<int> (),no_of_neighbors,indices,dists);
std::vector<int> k_indices;
k_indices.resize (no_of_neighbors);
for (std::size_t i = 0; i < cloud_rgb.points.size (); ++i)
{
- kdtree
->nearestKSearchT<pcl::PointXYZRGB> (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t);
- kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
+ kdtree
.nearestKSearchT<pcl::PointXYZRGB> (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t);
+ kdtree.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
EXPECT_EQ (k_indices.size (), indices[i].size ());
EXPECT_EQ (k_distances.size (), dists[i].size ());
for (std::size_t j=0; j< no_of_neighbors; j++)
{
unsigned int no_of_neighbors = 20;
- pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ> ();
- //kdtree->initSearchDS ();
- kdtree->setInputCloud (cloud_big.makeShared ());
+ pcl::search::KdTree<PointXYZ> kdtree;
+ //kdtree.initSearchDS ();
+ kdtree.setInputCloud (cloud_big.makeShared ());
std::vector< std::vector< float > > dists;
std::vector< std::vector< int > > indices;
- kdtree->nearestKSearch (cloud_big, std::vector<int> (),no_of_neighbors,indices,dists);
+ kdtree.nearestKSearch (cloud_big, std::vector<int> (),no_of_neighbors,indices,dists);
std::vector<int> k_indices;
k_indices.resize (no_of_neighbors);
for (std::size_t i = 0; i < cloud_big.points.size (); ++i)
{
- kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
+ kdtree.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
EXPECT_EQ (k_indices.size (), indices[i].size ());
EXPECT_EQ (k_distances.size (), dists[i].size ());
for (std::size_t j=0; j< no_of_neighbors; j++)
init ();
// Testing using explicit instantiation of inherited class
- pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ> ();
- kdtree->setInputCloud (cloud.makeShared ());
+ pcl::search::KdTree<PointXYZ> kdtree;
+ kdtree.setInputCloud (cloud.makeShared ());
return (RUN_ALL_TESTS ());
}
std::priority_queue<prioPointQueueEntry, pcl::PointCloud<prioPointQueueEntry>::VectorType> pointCandidates;
// create octree
- pcl::search::Search<PointXYZ>* octree = new pcl::search::Octree<PointXYZ> (0.1);
+ pcl::search::Octree<PointXYZ> octree(0.1);
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
pointCandidates.pop ();
}
// octree nearest neighbor search
- octree->setInputCloud (cloudIn);
- octree->nearestKSearch (searchPoint, static_cast<int> (K), k_indices, k_sqr_distances);
+ octree.setInputCloud (cloudIn);
+ octree.nearestKSearch (searchPoint, static_cast<int> (K), k_indices, k_sqr_distances);
ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
srand (time (NULL));
// create organized search
- pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
+ pcl::search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
k_sqr_distances.clear();
// organized nearest neighbor search
- organizedNeighborSearch->setInputCloud (cloudIn);
-// organizedNeighborSearch->nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances);
+ organizedNeighborSearch.setInputCloud (cloudIn);
+// organizedNeighborSearch.nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances);
k_indices_bruteforce.clear();
k_sqr_distances_bruteforce.clear();
srand (time (NULL));
// create organized search
- pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
+ pcl::search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
k_sqr_distances.clear();
// organized nearest neighbor search
- organizedNeighborSearch->setInputCloud (cloudIn);
- organizedNeighborSearch->nearestKSearch (searchIdx, (int)K, k_indices, k_sqr_distances);
+ organizedNeighborSearch.setInputCloud (cloudIn);
+ organizedNeighborSearch.nearestKSearch (searchIdx, (int)K, k_indices, k_sqr_distances);
k_indices_bruteforce.clear();
k_sqr_distances_bruteforce.clear();
srand (time (NULL));
- pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
+ pcl::search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
std::vector<int> cloudNWRSearch;
std::vector<float> cloudNWRRadius;
- organizedNeighborSearch->setInputCloud (cloudIn);
+ organizedNeighborSearch.setInputCloud (cloudIn);
- organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
+ organizedNeighborSearch.radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
// check if result from organized radius search can be also found in bruteforce search
ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
// check if result limitation works
- organizedNeighborSearch->radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
+ organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
ASSERT_EQ (cloudNWRRadius.size () <= 5, true);
}
constexpr unsigned int test_runs = 10;
srand (time (NULL));
- pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
+ pcl::search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch();
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
std::vector<float> cloudNWRRadius;
double check_time = getTime();
- organizedNeighborSearch->setInputCloud (cloudIn);
- organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
+ organizedNeighborSearch.setInputCloud (cloudIn);
+ organizedNeighborSearch.radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
double check_time2 = getTime();
radiusSearchLPTime += check_time2 - check_time;
- organizedNeighborSearch->setInputCloud (cloudIn);
- organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
+ organizedNeighborSearch.setInputCloud (cloudIn);
+ organizedNeighborSearch.radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
radiusSearchTime += getTime() - check_time2;
}
constexpr unsigned int test_runs = 10;
srand (time (NULL));
- pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
+ pcl::search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch();
// typical focal length from kinect
unsigned int randomIdx;
cloudNWRRadius2.clear();
double check_time = getTime();
- organizedNeighborSearch->setInputCloud (cloudIn);
- organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch2, cloudNWRRadius2, INT_MAX); //,INT_MAX);
+ organizedNeighborSearch.setInputCloud (cloudIn);
+ organizedNeighborSearch.radiusSearch (randomIdx, searchRadius, cloudNWRSearch2, cloudNWRRadius2, INT_MAX); //,INT_MAX);
double check_time2 = getTime();
sum_time+= check_time2 - check_time;
cloudNWRSearch.clear();
cloudNWRRadius.clear();
double check_time = getTime();
- organizedNeighborSearch->setInputCloud (cloudIn);
- organizedNeighborSearch->approxRadiusSearch (cloudIn, randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
+ organizedNeighborSearch.setInputCloud (cloudIn);
+ organizedNeighborSearch.approxRadiusSearch (cloudIn, randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
double check_time2 = getTime();
sum_time2+= check_time2 - check_time;
ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
// check if result limitation works
- organizedNeighborSearch->radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
+ organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
ASSERT_EQ (cloudNWRRadius.size () <= 5, true);
}
#include <pcl/search/organized.h>
#include <pcl/search/octree.h>
#include <pcl/io/pcd_io.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/common/time.h>
+
using namespace pcl;
using namespace std;
}
// remove also Nans
- #pragma omp parallel for
+ #pragma omp parallel for \
+ shared(nan_mask, point_cloud) \
+ default(none)
for (int pIdx = 0; pIdx < int (point_cloud->size ()); ++pIdx)
{
if (!isFinite (point_cloud->points [pIdx]))
if (!input_indices.empty ())
input_indices_.reset (new pcl::Indices (input_indices));
- #pragma omp parallel for
+ #pragma omp parallel for \
+ shared(input_indices, input_indices_, point_cloud, search_methods) \
+ default(none)
for (int sIdx = 0; sIdx < int (search_methods.size ()); ++sIdx)
search_methods [sIdx]->setInputCloud (point_cloud, input_indices_);
// find nn for each point in the cloud
for (const int &query_index : query_indices)
{
- #pragma omp parallel for
+ #pragma omp parallel for \
+ shared(indices, input_indices, indices_mask, distances, knn, nan_mask, passed, point_cloud, query_index, search_methods) \
+ default(none)
for (int sIdx = 0; sIdx < int (search_methods.size ()); ++sIdx)
{
search_methods [sIdx]->nearestKSearch (point_cloud->points[query_index], knn, indices [sIdx], distances [sIdx]);
}
// compare results to each other
- #pragma omp parallel for
+ #pragma omp parallel for \
+ shared(distances, indices, passed, search_methods) \
+ default(none)
for (int sIdx = 1; sIdx < int (search_methods.size ()); ++sIdx)
{
passed [sIdx] = passed [sIdx] && compareResults (indices [0], distances [0], search_methods [0]->getName (),
}
// remove also Nans
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none) \
+ shared(nan_mask, point_cloud)
for (int pIdx = 0; pIdx < int (point_cloud->size ()); ++pIdx)
{
if (!isFinite (point_cloud->points [pIdx]))
if (!input_indices.empty ())
input_indices_.reset (new pcl::Indices (input_indices));
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none) \
+ shared(input_indices_, point_cloud, search_methods)
for (int sIdx = 0; sIdx < int (search_methods.size ()); ++sIdx)
search_methods [sIdx]->setInputCloud (point_cloud, input_indices_);
// find nn for each point in the cloud
for (const int &query_index : query_indices)
{
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none) \
+ shared(distances, indices, indices_mask, input_indices, nan_mask, passed, point_cloud, radius, query_index, search_methods)
for (int sIdx = 0; sIdx < static_cast<int> (search_methods.size ()); ++sIdx)
{
search_methods [sIdx]->radiusSearch (point_cloud->points[query_index], radius, indices [sIdx], distances [sIdx], 0);
}
// compare results to each other
- #pragma omp parallel for
+ #pragma omp parallel for \
+ default(none) \
+ shared(distances, indices, passed, search_methods)
for (int sIdx = 1; sIdx < static_cast<int> (search_methods.size ()); ++sIdx)
{
passed [sIdx] = passed [sIdx] && compareResults (indices [0], distances [0], search_methods [0]->getName (),
float frac_of_points_for_registration = 0.3f;
std::string object_name = "test_object";
- ModelLibrary::Model* new_model = new ModelLibrary::Model (*model_cloud, *model_cloud_normals, voxel_size, object_name, frac_of_points_for_registration);
+ ModelLibrary::Model new_model (*model_cloud, *model_cloud_normals, voxel_size, object_name, frac_of_points_for_registration);
- const ORROctree& octree = new_model->getOctree ();
+ const ORROctree& octree = new_model.getOctree ();
const std::vector<ORROctree::Node*> &full_leaves = octree.getFullLeaves ();
list<ORROctree::Node*> inter_leaves;
EXPECT_NE(*leaf1, *leaf2);
}
}
-
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include <pcl/test/gtest.h>
+#include <pcl/common/generate.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
search::KdTree<PointXYZ>::Ptr tree3;
search::KdTree<PointNormal>::Ptr tree4;
+// Test that updatepointcloud works when removing points. Ie. modifying vtk data structure to reflect modified pointcloud
+// See #4001 and #3452 for previously undetected error.
+////////////////////////////////////////////////////////////////////////////////
+TEST(PCL, PCLVisualizer_updatePointCloud)
+{
+ pcl::common::CloudGenerator<pcl::PointXYZRGB, pcl::common::UniformGenerator<float> > generator;
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+ int result = generator.fill(3, 1, *cloud);
+
+ // Setup a basic viewport window
+ pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
+ pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color_green(cloud, 0, 225, 100);
+ viewer->addPointCloud<pcl::PointXYZRGB>(cloud, color_green, "sample cloud");
+
+ // remove points one by one)
+ while (!cloud->empty()) {
+ cloud->erase(cloud->end() - 1);
+ viewer->updatePointCloud(cloud, "sample cloud");
+ viewer->spinOnce(100);
+ }
+}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, PCLVisualizer_camera)
{
PCL_ADD_EXECUTABLE(pcl_pcd2png COMPONENT ${SUBSYS_NAME} SOURCES pcd2png.cpp)
target_link_libraries(pcl_pcd2png pcl_common pcl_io)
- PCL_ADD_EXECUTABLE(pcl_organized_pcd_to_png COMPONENT ${SUBSYS_NAME} SOURCES organized_pcd_to_png.cpp)
- target_link_libraries (pcl_organized_pcd_to_png pcl_common pcl_io)
-
PCL_ADD_EXECUTABLE(pcl_tiff2pcd COMPONENT ${SUBSYS_NAME} SOURCES tiff2pcd.cpp)
target_link_libraries(pcl_tiff2pcd pcl_common pcl_io)
#ifndef Q_MOC_RUN
// Marking all Boost headers as system headers to remove warnings
-#include <boost/make_shared.hpp>
#include <boost/date_time/gregorian/gregorian_types.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Willow Garage, Inc.
- *
+ *
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
*/
#include <pcl/PCLPointCloud2.h>
+#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
+
#include <vector>
-#include "boost.h"
using namespace pcl;
using namespace pcl::io;
{
print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
print_info (" where options are:\n");
- print_info (" -min X = use a minimum of X points peer cluster (default: ");
+ print_info (" -min X = use a minimum of X points peer cluster (default: ");
print_value ("%d", default_min); print_info (")\n");
- print_info (" -max X = use a maximum of X points peer cluster (default: ");
+ print_info (" -max X = use a maximum of X points peer cluster (default: ");
print_value ("%d", default_max); print_info (")\n");
- print_info (" -tolerance X = the spacial distance between clusters (default: ");
+ print_info (" -tolerance X = the spacial distance between clusters (default: ");
print_value ("%lf", default_tolerance); print_info (")\n");
}
// Estimate
TicToc tt;
tt.tic ();
-
+
print_highlight (stderr, "Computing ");
// Creating the KdTree object for the search method of the extraction
{
pcl::ExtractIndices<pcl::PCLPointCloud2> extract;
extract.setInputCloud (input);
- extract.setIndices (boost::make_shared<const pcl::PointIndices> (*it));
+ extract.setIndices (pcl::make_shared<const pcl::PointIndices> (*it));
pcl::PCLPointCloud2::Ptr out (new pcl::PCLPointCloud2);
extract.filter (*out);
output.push_back (out);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output[i]->width * output[i]->height); print_info (" points]\n");
}
-
+
}
/* ---[ */
// Load the first file
pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
- if (!loadCloud (argv[p_file_indices[0]], *cloud))
+ if (!loadCloud (argv[p_file_indices[0]], *cloud))
return (-1);
// Perform the feature estimation
*
*/
+#include <pcl/memory.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/io/depth_sense_grabber.h>
#include <pcl/visualization/pcl_visualizer.h>
-#include <boost/shared_ptr.hpp>
#include <boost/format.hpp>
#include <iostream>
int
batchProcess (const std::vector<string> &pcd_files, string &output_dir, float sigma_s, float sigma_r)
{
-#if _OPENMP
-#pragma omp parallel for
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(output_dir, pcd_files, sigma_r, sigma_s)
for (int i = 0; i < int (pcd_files.size ()); ++i)
{
// Load the first file
int
batchProcess (const std::vector<string> &pcd_files, string &output_dir, int k, double radius)
{
-#if _OPENMP
-#pragma omp parallel for
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(k, output_dir, pcd_files, radius)
for (int i = 0; i < int (pcd_files.size ()); ++i)
{
// Load the first file
pcl::TimeTrigger trigger;
- pcl::ONIGrabber* grabber = nullptr;
- if (frame_rate == 0)
- grabber = new pcl::ONIGrabber(arg, true, true);
- else
+ pcl::ONIGrabber grabber (arg, true, frame_rate == 0);
+ if (frame_rate != 0)
{
- grabber = new pcl::ONIGrabber(arg, true, false);
trigger.setInterval (1.0 / static_cast<double> (frame_rate));
- trigger.registerCallback ([=] { grabber->start (); });
+ trigger.registerCallback ([&] { grabber.start (); });
trigger.start();
}
- if (grabber
->providesCallback<pcl::ONIGrabber::sig_cb_openni_point_cloud_rgb > () && !pcl::console::find_switch (argc, argv, "-xyz"))
+ if (grabber
.providesCallback<pcl::ONIGrabber::sig_cb_openni_point_cloud_rgb > () && !pcl::console::find_switch (argc, argv, "-xyz"))
{
- SimpleONIViewer<pcl::PointXYZRGBA> v(*grabber);
+ SimpleONIViewer<pcl::PointXYZRGBA> v(grabber);
v.run();
}
else
{
- SimpleONIViewer<pcl::PointXYZ> v(*grabber);
+ SimpleONIViewer<pcl::PointXYZ> v(grabber);
v.run();
}
cloud_connection.disconnect();
image_connection.disconnect();
- if (rgb_data)
- delete[] rgb_data;
+ delete[] rgb_data;
}
pcl::visualization::CloudViewer cloud_viewer_;
+++ /dev/null
-/*
- * Software License Agreement (BSD License)
- *
- * Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2013, Open Perception, Inc.
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the copyright holder(s) nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- */
-
-#include <pcl/io/pcd_io.h>
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
-#include <pcl/console/time.h>
-#include <pcl/io/png_io.h>
-
-using namespace pcl;
-using namespace pcl::io;
-using namespace pcl::console;
-
-void
-printHelp (int, char **argv)
-{
- print_error ("Syntax is: %s input.pcd output.png\n", argv[0]);
-}
-
-bool
-loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud)
-{
- TicToc tt;
- print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
-
- tt.tic ();
- if (loadPCDFile (filename, cloud) < 0)
- return (false);
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
- print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
-
- return (true);
-}
-
-void
-saveImage (const std::string &filename, const PointCloud<RGB> &image)
-{
- TicToc tt;
- tt.tic ();
-
- print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
- io::savePNGFile (filename, image, "rgb");
-
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", image.width * image.height); print_info (" points]\n");
-}
-
-/* ---[ */
-int
-main (int argc, char** argv)
-{
- print_error ("This tool is deprecated, please use \"pcl_pcd2png\" instead!\n");
- print_info ("Convert the RGB information of an organized PCD file to a PNG image. For more information, use: %s -h\n", argv[0]);
-
- if (argc < 3)
- {
- printHelp (argc, argv);
- return (-1);
- }
-
- // Parse the command line arguments for .pcd files
- std::vector<int> pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
- if (pcd_file_indices.size () != 1)
- {
- print_error ("Need one input PCD file and one output PNG file.\n");
- return (-1);
- }
-
- std::vector<int> png_file_indices = parse_file_extension_argument (argc, argv, ".png");
- if (png_file_indices.size () != 1)
- {
- print_error ("Need one input PCD file and one output PNG file.\n");
- return (-1);
- }
-
- // Load the first file
- pcl::PCLPointCloud2 cloud;
- if (!loadCloud (argv[pcd_file_indices[0]], cloud))
- return (-1);
-
-
- PointCloud<RGB> image;
- fromPCLPointCloud2 (cloud, image);
-
-
- // Check if the cloud is organized
- if (!image.isOrganized ())
- {
- PCL_ERROR ("Input cloud is not organized.\n");
- return (-1);
- }
-
- // Save cloud
- saveImage (argv[png_file_indices[0]], image);
-
- return (0);
-}
-
*
*/
+#include <pcl/memory.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/format.hpp>
-#include <boost/shared_ptr.hpp>
#include <iostream>
#include <mutex>
* \author Radu Bogdan Rusu
*
* @b virtual_scanner takes in a .ply or a .vtk file of an object model, and virtually scans it
- * in a raytracing fashion, saving the end results as PCD (Point Cloud Data) files. In addition,
- * it noisifies the PCD models, and downsamples them.
+ * in a raytracing fashion, saving the end results as PCD (Point Cloud Data) files. In addition,
+ * it noisifies the PCD models, and downsamples them.
* The viewpoint can be set to 1 or multiple views on a sphere.
*/
#include <random>
#include <string>
+
#include <pcl/register_point_struct.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/filters/voxel_grid.h>
+#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/vtk.h>
-#include "boost.h"
+
+#include <boost/algorithm/string.hpp> // for boost::is_any_of, boost::split, boost::token_compress_on, boost::trim
+#include <boost/filesystem.hpp> // for boost::filesystem::create_directories, boost::filesystem::exists, boost::filesystem::extension, boost::filesystem::path
using namespace pcl;
PCL_ERROR ("Error: no .PLY or .VTK files given!\n");
return (-1);
}
-
+
std::stringstream filename_stream;
if (!p_file_indices_ply.empty ())
filename_stream << argv[p_file_indices_ply.at (0)];
filename_stream << argv[p_file_indices_vtk.at (0)];
filename = filename_stream.str ();
-
+
data = loadDataSet (filename.c_str ());
-
+
PCL_INFO ("Loaded model with %d vertices/points.\n", data->GetNumberOfPoints ());
// Default scan parameters
double temp_beam[3], beam[3], p[3];
double p_coords[3], x[3], t;
int subId;
-
+
// Create a Icosahedron at center in origin and radius of 1
vtkSmartPointer<vtkPlatonicSolidSource> icosa = vtkSmartPointer<vtkPlatonicSolidSource>::New ();
icosa->SetSolidTypeToIcosahedron ();
up[0] /= up_len;
up[1] /= up_len;
up[2] /= up_len;
-
+
// Output resulting vectors
std::cerr << "Viewray Right Up:" << std::endl;
std::cerr << viewray[0] << " " << viewray[1] << " " << viewray[2] << " " << std::endl;
for (double hor = hor_start; hor <= hor_end; hor += sp.hor_res)
{
pid ++;
-
+
// Create a beam vector with (lat,long) angles (vert, hor) with the viewray
tr2->Identity ();
tr2->RotateWXYZ (hor, up);
pcl::PointWithViewpoint pt;
if (object_coordinates)
{
- pt.x = static_cast<float> (x[0]);
- pt.y = static_cast<float> (x[1]);
+ pt.x = static_cast<float> (x[0]);
+ pt.y = static_cast<float> (x[1]);
pt.z = static_cast<float> (x[2]);
- pt.vp_x = static_cast<float> (eye[0]);
- pt.vp_y = static_cast<float> (eye[1]);
+ pt.vp_x = static_cast<float> (eye[0]);
+ pt.vp_y = static_cast<float> (eye[1]);
pt.vp_z = static_cast<float> (eye[2]);
}
else
// Downsample and remove silly point duplicates
pcl::PointCloud<pcl::PointWithViewpoint> cloud_downsampled;
- grid.setInputCloud (
boost::make_shared<pcl::PointCloud<pcl::PointWithViewpoint> > (cloud));
+ grid.setInputCloud (
pcl::make_shared<pcl::PointCloud<pcl::PointWithViewpoint> > (cloud));
//grid.filter (cloud_downsampled);
// Saves the point cloud data to disk
#include <pcl/pcl_macros.h>
-PCL_PRAGMA_WARNING("This header is deprecated and will be removed in an upcoming release.")
+PCL_DEPRECATED_HEADER(1, 12, "")
#include <boost/random.hpp>
#pragma once
-#include <boost/shared_ptr.hpp>
-
+#include <pcl/memory.h>
#include <pcl/tracking/coherence.h>
+
namespace pcl
{
namespace tracking
{
if (!use_normal_)
{
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
this->computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
change_counter_ = change_detector_interval_;
coherence_->setTargetCloud (coherence_input);
coherence_->initCompute ();
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
{
IndicesPtr indices;
--change_counter_;
coherence_->setTargetCloud (coherence_input);
coherence_->initCompute ();
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
{
IndicesPtr indices;
{
indices_list[i] = IndicesPtr (new std::vector<int>);
}
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(indices_list) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
{
this->computeTransformedPointCloudWithNormal (particles_->points[i], *indices_list[i], *transed_reference_vector_[i]);
coherence_->setTargetCloud (coherence_input);
coherence_->initCompute ();
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(indices_list) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
{
coherence_->compute (transed_reference_vector_[i], indices_list[i], particles_->points[i].weight);
{
if (!use_normal_)
{
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
this->computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
change_counter_ = change_detector_interval_;
coherence_->setTargetCloud (coherence_input);
coherence_->initCompute ();
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
{
IndicesPtr indices; // dummy
--change_counter_;
coherence_->setTargetCloud (coherence_input);
coherence_->initCompute ();
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
{
IndicesPtr indices; // dummy
{
indices_list[i] = IndicesPtr (new std::vector<int>);
}
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(indices_list) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
{
this->computeTransformedPointCloudWithNormal (particles_->points[i], *indices_list[i], *transed_reference_vector_[i]);
coherence_->setTargetCloud (coherence_input);
coherence_->initCompute ();
-#ifdef _OPENMP
-#pragma omp parallel for num_threads(threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(indices_list) \
+ num_threads(threads_)
for (int i = 0; i < particle_num_; i++)
{
coherence_->compute (transed_reference_vector_[i], indices_list[i], particles_->points[i].weight);
#include <pcl/common/io.h>
#include <pcl/common/utils.h>
-///////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace tracking
+{
+
template <typename PointInT, typename IntensityT> inline void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::setTrackingWindowSize (int width, int height)
+PyramidalKLTTracker<PointInT, IntensityT>::setTrackingWindowSize (int width, int height)
{
track_width_ = width;
track_height_ = height;
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> inline void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::setPointsToTrack (const pcl::PointCloud<pcl::PointUV>::ConstPtr& keypoints)
+PyramidalKLTTracker<PointInT, IntensityT>::setPointsToTrack (const pcl::PointCloud<pcl::PointUV>::ConstPtr& keypoints)
{
if (keypoints->size () <= keypoints_nbr_)
keypoints_ = keypoints;
keypoints_status_->indices.resize (keypoints_->size (), 0);
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> inline void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::setPointsToTrack (const pcl::PointIndicesConstPtr& points)
+PyramidalKLTTracker<PointInT, IntensityT>::setPointsToTrack (const pcl::PointIndicesConstPtr& points)
{
assert ((input_ || ref_) && "[pcl::tracking::PyramidalKLTTracker] CALL setInputCloud FIRST!");
setPointsToTrack (keypoints);
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> bool
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::initCompute ()
+PyramidalKLTTracker<PointInT, IntensityT>::initCompute ()
{
// std::cout << ">>> [PyramidalKLTTracker::initCompute]" << std::endl;
if (!PCLBase<PointInT>::initCompute ())
computePyramids (ref_, ref_pyramid_, pcl::BORDER_REFLECT_101);
return (true);
}
+
initialized_ = true;
return (true);
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::derivatives (const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const
+PyramidalKLTTracker<PointInT, IntensityT>::derivatives (const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const
{
// std::cout << ">>> derivatives" << std::endl;
////////////////////////////////////////////////////////
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::downsample (const FloatImageConstPtr& input,
+PyramidalKLTTracker<PointInT, IntensityT>::downsample (const FloatImageConstPtr& input,
FloatImageConstPtr& output) const
{
FloatImage smoothed (input->width, input->height);
ii[i] = 2 * i;
FloatImagePtr down (new FloatImage (width, height));
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) firstprivate (ii) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(down, height, output, smoothed, width) \
+ firstprivate(ii) \
+ num_threads(threads_)
for (int j = 0; j < height; ++j)
{
int jj = 2*j;
output = down;
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::downsample (const FloatImageConstPtr& input,
- FloatImageConstPtr& output,
- FloatImageConstPtr& output_grad_x,
- FloatImageConstPtr& output_grad_y) const
+PyramidalKLTTracker<PointInT, IntensityT>::downsample (const FloatImageConstPtr& input,
+ FloatImageConstPtr& output,
+ FloatImageConstPtr& output_grad_x,
+ FloatImageConstPtr& output_grad_y) const
{
downsample (input, output);
FloatImagePtr grad_x (new FloatImage (input->width, input->height));
output_grad_y = grad_y;
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::convolve (const FloatImageConstPtr& input, FloatImage& output) const
+PyramidalKLTTracker<PointInT, IntensityT>::convolve (const FloatImageConstPtr& input, FloatImage& output) const
{
FloatImagePtr tmp (new FloatImage (input->width, input->height));
convolveRows (input, *tmp);
convolveCols (tmp, output);
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::convolveRows (const FloatImageConstPtr& input, FloatImage& output) const
+PyramidalKLTTracker<PointInT, IntensityT>::convolveRows (const FloatImageConstPtr& input, FloatImage& output) const
{
int width = input->width;
int height = input->height;
int last = input->width - kernel_size_2_;
int w = last - 1;
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(input, height, last, output, w, width) \
+ num_threads(threads_)
for (int j = 0; j < height; ++j)
{
for (int i = kernel_size_2_; i < last; ++i)
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::convolveCols (const FloatImageConstPtr& input, FloatImage& output) const
+PyramidalKLTTracker<PointInT, IntensityT>::convolveCols (const FloatImageConstPtr& input, FloatImage& output) const
{
output = FloatImage (input->width, input->height);
int last = input->height - kernel_size_2_;
int h = last -1;
-#ifdef _OPENMP
-#pragma omp parallel for shared (output) num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(input, h, height, last, output, width) \
+ num_threads(threads_)
for (int i = 0; i < width; ++i)
{
for (int j = kernel_size_2_; j < last; ++j)
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::computePyramids (const PointCloudInConstPtr& input,
- std::vector<FloatImageConstPtr>& pyramid,
- pcl::InterpolationType border_type) const
+PyramidalKLTTracker<PointInT, IntensityT>::computePyramids (const PointCloudInConstPtr& input,
+ std::vector<FloatImageConstPtr>& pyramid,
+ pcl::InterpolationType border_type) const
{
int step = 3;
pyramid.resize (step * nb_levels_);
FloatImageConstPtr previous;
FloatImagePtr tmp (new FloatImage (input->width, input->height));
-#ifdef _OPENMP
-#pragma omp parallel for num_threads (threads_)
-#endif
+#pragma omp parallel for \
+ default(none) \
+ shared(input, tmp) \
+ num_threads(threads_)
for (int i = 0; i < static_cast<int> (input->size ()); ++i)
tmp->points[i] = intensity_ (input->points[i]);
previous = tmp;
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::spatialGradient (const FloatImage& img,
- const FloatImage& grad_x,
- const FloatImage& grad_y,
- const Eigen::Array2i& location,
- const Eigen::Array4f& weight,
- Eigen::ArrayXXf& win,
- Eigen::ArrayXXf& grad_x_win,
- Eigen::ArrayXXf& grad_y_win,
- Eigen::Array3f &covariance) const
+PyramidalKLTTracker<PointInT, IntensityT>::spatialGradient (const FloatImage& img,
+ const FloatImage& grad_x,
+ const FloatImage& grad_y,
+ const Eigen::Array2i& location,
+ const Eigen::Array4f& weight,
+ Eigen::ArrayXXf& win,
+ Eigen::ArrayXXf& grad_x_win,
+ Eigen::ArrayXXf& grad_y_win,
+ Eigen::Array3f &covariance) const
{
const int step = img.width;
covariance.setZero ();
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::mismatchVector (const Eigen::ArrayXXf& prev,
- const Eigen::ArrayXXf& prev_grad_x,
- const Eigen::ArrayXXf& prev_grad_y,
- const FloatImage& next,
- const Eigen::Array2i& location,
- const Eigen::Array4f& weight,
- Eigen::Array2f &b) const
+PyramidalKLTTracker<PointInT, IntensityT>::mismatchVector (const Eigen::ArrayXXf& prev,
+ const Eigen::ArrayXXf& prev_grad_x,
+ const Eigen::ArrayXXf& prev_grad_y,
+ const FloatImage& next,
+ const Eigen::Array2i& location,
+ const Eigen::Array4f& weight,
+ Eigen::Array2f &b) const
{
const int step = next.width;
b.setZero ();
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::track (const PointCloudInConstPtr& prev_input,
- const PointCloudInConstPtr& input,
- const std::vector<FloatImageConstPtr>& prev_pyramid,
- const std::vector<FloatImageConstPtr>& pyramid,
-
const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
-
pcl::PointCloud<pcl::PointUV>::Ptr& keypoints,
- std::vector<int>& status,
- Eigen::Affine3f& motion) const
+PyramidalKLTTracker<PointInT, IntensityT>::track (const PointCloudInConstPtr& prev_input,
+ const PointCloudInConstPtr& input,
+ const std::vector<FloatImageConstPtr>& prev_pyramid,
+ const std::vector<FloatImageConstPtr>& pyramid,
+ const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
+ pcl::PointCloud<pcl::PointUV>::Ptr& keypoints,
+ std::vector<int>& status,
+ Eigen::Affine3f& motion) const
{
std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f> > next_pts (prev_keypoints->size ());
Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f);
}
}
}
+
motion = transformation_computer.getTransformation ();
}
-///////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointInT, typename IntensityT> void
-
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::computeTracking ()
+PyramidalKLTTracker<PointInT, IntensityT>::computeTracking ()
{
if (!initialized_)
return;
keypoints_status_->indices = status;
}
+} // namespace tracking
+} // namespace pcl
+
#endif
+
#ifndef PCL_TRACKING_IMPL_TRACKING_H_
#define PCL_TRACKING_IMPL_TRACKING_H_
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/common/eigen.h>
#include <ctime>
#pragma once
-#include <boost/shared_ptr.hpp>
-
-#include <pcl/tracking/tracking.h>
-#include <pcl/tracking/tracker.h>
-#include <pcl/tracking/coherence.h>
+#include <pcl/memory.h>
#include <pcl/filters/passthrough.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
+#include <pcl/tracking/coherence.h>
+#include <pcl/tracking/tracker.h>
+#include <pcl/tracking/tracking.h>
#include <Eigen/Dense>
+
namespace pcl
{
namespace tracking
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/tracking/tracker.h>
#pragma once
#include <pcl/tracking/tracking.h>
+#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <pcl/search/search.h>
{
protected:
using PCLBase<PointInT>::deinitCompute;
-
+
public:
using PCLBase<PointInT>::indices_;
using PCLBase<PointInT>::input_;
-
+
using BaseClass = PCLBase<PointInT>;
using Ptr = shared_ptr< Tracker<PointInT, StateT> >;
using ConstPtr = shared_ptr< const Tracker<PointInT, StateT> >;
using PointCloudIn = pcl::PointCloud<PointInT>;
using PointCloudInPtr = typename PointCloudIn::Ptr;
using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
-
+
using PointCloudState = pcl::PointCloud<StateT>;
using PointCloudStatePtr = typename PointCloudState::Ptr;
using PointCloudStateConstPtr = typename PointCloudState::ConstPtr;
-
+
public:
/** \brief Empty constructor. */
Tracker (): search_ () {}
-
- /** \brief Base method for tracking for all points given in
- * <setInputCloud (), setIndices ()> using the indices in setIndices ()
+
+ /** \brief Base method for tracking for all points given in
+ * <setInputCloud (), setIndices ()> using the indices in setIndices ()
*/
- void
+ void
compute ();
-
+
protected:
/** \brief The tracker name. */
std::string tracker_name_;
SearchPtr search_;
/** \brief Get a string representation of the name of this class. */
- inline const std::string&
+ inline const std::string&
getClassName () const { return (tracker_name_); }
/** \brief This method should get called before starting the actual computation. */
* to estimate the features for every point in the input dataset. This
* is optional, if this is not set, it will only use the data in the
* input cloud to estimate the features. This is useful when you only
- * need to compute the features for a downsampled cloud.
+ * need to compute the features for a downsampled cloud.
* \param search a pointer to a PointCloud message
*/
- inline void
+ inline void
setSearchMethod (const SearchPtr &search) { search_ = search; }
/** \brief Get a pointer to the point cloud dataset. */
- inline SearchPtr
+ inline SearchPtr
getSearchMethod () { return (search_); }
-
+
/** \brief Get an instance of the result of tracking. */
- virtual StateT
+ virtual StateT
getResult () const = 0;
-
+
private:
/** \brief Abstract tracking method. */
virtual void
computeTracking () = 0;
-
+
public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
#include <pcl/point_types.h>
-#ifdef BUILD_Maintainer
-# if defined __GNUC__
-# pragma GCC system_header
-# elif defined _MSC_VER
-# pragma warning(push, 1)
-# endif
-#endif
-
namespace pcl
{
namespace tracking
)
POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleXYZR, pcl::tracking::_ParticleXYZR)
-#ifdef BUILD_Maintainer
-# if defined _MSC_VER
-# pragma warning(pop)
-# endif
-#endif
-
#ifdef PCL_NO_PRECOMPILE
#include <pcl/tracking/impl/tracking.hpp>
#endif
if(BUILD_TESTS)
add_subdirectory(test)
endif()
-
#include <boost/shared_array.hpp>
#define BOOST_PARAMETER_MAX_ARITY 7
#include <boost/signals2.hpp>
-#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
PCL_VISUALIZER_IMMEDIATE_RENDERING,
PCL_VISUALIZER_SHADING,
PCL_VISUALIZER_LUT, /**< colormap type \ref pcl::visualization::LookUpTableRepresentationProperties */
- PCL_VISUALIZER_LUT_RANGE /**< two doubles (min and max) or \ref pcl::visualization::LookUpTableRepresentationProperties::PCL_VISUALIZER_LUT_RANGE_AUTO */
+ PCL_VISUALIZER_LUT_RANGE /**< two doubles (min and max) or ::PCL_VISUALIZER_LUT_RANGE_AUTO */
};
enum RenderingRepresentationProperties
* \param[in] colormap_type
* \param[out] table a vtk lookup table
* \note The list of available colormaps can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
- */
+ */
PCL_EXPORTS bool
getColormapLUT (LookUpTableRepresentationProperties colormap_type, vtkSmartPointer<vtkLookupTable> &table);
/** \brief Computes View matrix for Camera (Based on gluLookAt)
* \param[out] view_mat the resultant matrix
*/
- void
+ void
computeViewMatrix (Eigen::Matrix4d& view_mat) const;
/** \brief Computes Projection Matrix for Camera
* \param[out] proj the resultant matrix
*/
- void
+ void
computeProjectionMatrix (Eigen::Matrix4d& proj) const;
- /** \brief converts point to window coordiantes
- * \param[in] pt xyz point to be converted
- * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
- *
- * This function computes the projection and view matrix every time.
- * It is very inefficient to use this for every point in the point cloud!
- */
- template<typename PointT> void
+ /**
+ * \brief Converts point to window coordinates
+ * \param[in] pt xyz point to be converted
+ * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
+ * \note This function computes the projection and view matrix every time.
+ * It is very inefficient to use this for every point in the point cloud!
+ */
+ template<typename PointT> void
cvtWindowCoordinates (const PointT& pt, Eigen::Vector4d& window_cord) const;
- /** \brief converts point to window coordiantes
- * \param[in] pt xyz point to be converted
- * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
- * \param[in] composite_mat composite transformation matrix (proj*view)
- *
- * Use this function to compute window coordinates with a precomputed
- * transformation function. The typical composite matrix will be
- * the projection matrix * the view matrix. However, additional
- * matrices like a camera disortion matrix can also be added.
- */
- template<typename PointT> void
+ /**
+ * \brief Converts point to window coordinates
+ * \param[in] pt xyz point to be converted
+ * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
+ * \param[in] composite_mat composite transformation matrix (proj*view)
+ *
+ * \note Use this function to compute window coordinates with a pre-computed transformation function.
+ * The typical composite matrix will be the projection matrix * the view matrix. However, additional matrices like
+ * a camera distortion matrix can also be added.
+ */
+ template<typename PointT> void
cvtWindowCoordinates (const PointT& pt, Eigen::Vector4d& window_cord, const Eigen::Matrix4d& composite_mat) const;
};
}
{
namespace visualization
{
- /** \brief Converts point to window coordinates
- * \param[in] pt xyz point to be converted
- * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
- * \note This function computes the projection and view matrix every time.
- * It is very inefficient to use this for every point in the point cloud!
- */
template<typename PointT> void
Camera::cvtWindowCoordinates (const PointT& pt, Eigen::Vector4d& window_cord) const
{
return;
}
-
- /** \brief converts point to window coordiantes
- * \param[in] pt xyz point to be converted
- * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
- * \param[in] composite_mat composite transformation matrix (proj*view)
- *
- * \note Use this function to compute window coordinates with a pre-computed transformation function.
- * The typical composite matrix will be the projection matrix * the view matrix. However, additional matrices like
- * a camera distortion matrix can also be added.
- */
template<typename PointT> void
Camera::cvtWindowCoordinates (const PointT& pt, Eigen::Vector4d& window_cord, const Eigen::Matrix4d& composite_mat) const
{
*/
#pragma once
+
#include <vtkSmartPointer.h>
#include <vtkPoints.h>
#include <vtkPolygon.h>
#include <vtkUnstructuredGrid.h>
-////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace visualization
+{
+
template <typename PointT> vtkSmartPointer<vtkDataSet>
-
pcl::visualization::createPolygon (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
+createPolygon (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
{
vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
if (cloud->points.empty ())
return (poly_grid);
}
-////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> vtkSmartPointer<vtkDataSet>
-
pcl::visualization::createPolygon (const pcl::PlanarPolygon<PointT> &planar_polygon)
+createPolygon (const pcl::PlanarPolygon<PointT> &planar_polygon)
{
vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
if (planar_polygon.getContour ().empty ())
for (std::size_t i = 0; i < planar_polygon.getContour ().size (); ++i)
{
- poly_points->SetPoint (i, planar_polygon.getContour ()[i].x,
- planar_polygon.getContour ()[i].y,
+ poly_points->SetPoint (i, planar_polygon.getContour ()[i].x,
+ planar_polygon.getContour ()[i].y,
planar_polygon.getContour ()[i].z);
polygon->GetPointIds ()->SetId (i, i);
}
std::size_t closingContourId = planar_polygon.getContour ().size ();
auto firstContour = planar_polygon.getContour ()[0];
- poly_points->SetPoint (closingContourId, firstContour.x,
- firstContour.y,
+ poly_points->SetPoint (closingContourId, firstContour.x,
+ firstContour.y,
firstContour.z);
polygon->GetPointIds ()->SetId (closingContourId, closingContourId);
-
+
allocVtkUnstructuredGrid (poly_grid);
poly_grid->Allocate (1, 1);
poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
return (poly_grid);
}
+} // namespace visualization
+} // namespace pcl
+
#pragma once
+#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/console/print.h>
#include <vtkDoubleArray.h>
-////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace visualization
+{
+
template <typename PointT> bool
-pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
- const pcl::PointCloud<PointT> &cloud, int hsize,
+PCLHistogramVisualizer::addFeatureHistogram (
+ const pcl::PointCloud<PointT> &cloud, int hsize,
const std::string &id, int win_width, int win_height)
{
RenWinInteractMap::iterator am_it = wins_.find (id);
return (true);
}
-////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
- const pcl::PointCloud<PointT> &cloud,
+PCLHistogramVisualizer::addFeatureHistogram (
+ const pcl::PointCloud<PointT> &cloud,
const std::string &field_name,
- const int index,
+ const int index,
const std::string &id, int win_width, int win_height)
{
if (index < 0 || index >= cloud.points.size ())
return (true);
}
-////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
- const pcl::PointCloud<PointT> &cloud, int hsize,
+PCLHistogramVisualizer::updateFeatureHistogram (
+ const pcl::PointCloud<PointT> &cloud, int hsize,
const std::string &id)
{
RenWinInteractMap::iterator am_it = wins_.find (id);
return (false);
}
RenWinInteract* renwinupd = &wins_[id];
-
+
vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
xy_array->SetNumberOfComponents (2);
xy_array->SetNumberOfTuples (hsize);
-
+
// Parse the cloud data and store it in the array
double xy[2];
for (int d = 0; d < hsize; ++d)
return (true);
}
-////////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> bool
-pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
- const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const int index,
+PCLHistogramVisualizer::updateFeatureHistogram (
+ const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const int index,
const std::string &id)
{
if (index < 0 || index >= cloud.points.size ())
PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index);
return (false);
}
-
+
// Get the fields present in this cloud
std::vector<pcl::PCLPointField> fields;
// Check if our field exists
return (false);
}
RenWinInteract* renwinupd = &wins_[id];
-
+
vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
xy_array->SetNumberOfComponents (2);
xy_array->SetNumberOfTuples (fields[field_idx].count);
xy[1] = data;
xy_array->SetTuple (d, xy);
}
-
+
reCreateActor (xy_array, renwinupd, cloud.fields[field_idx].count - 1);
return (true);
}
+} // namespace visualization
+} // namespace pcl
+
#endif
#ifndef PCL_VISUALUALIZATION_PCL_PLOTTER_IMPL_H_
#define PCL_VISUALUALIZATION_PCL_PLOTTER_IMPL_H_
+
+namespace pcl
+{
+
+namespace visualization
+{
+
template <typename PointT> bool
-pcl::visualization::PCLPlotter::addFeatureHistogram (
- const pcl::PointCloud<PointT> &cloud, int hsize,
+PCLPlotter::addFeatureHistogram (
+ const pcl::PointCloud<PointT> &cloud, int hsize,
const std::string &id, int win_width, int win_height)
{
std::vector<double> array_x(hsize), array_y(hsize);
-
+
// Parse the cloud data and store it in the array
for (int i = 0; i < hsize; ++i)
{
array_x[i] = i;
array_y[i] = cloud.points[0].histogram[i];
}
-
+
this->addPlotData(array_x, array_y, id.c_str(), vtkChart::LINE);
setWindowSize (win_width, win_height);
return true;
template <typename PointT> bool
-pcl::visualization::PCLPlotter::addFeatureHistogram (
- const pcl::PointCloud<PointT> &cloud,
+PCLPlotter::addFeatureHistogram (
+ const pcl::PointCloud<PointT> &cloud,
const std::string &field_name,
- const int index,
+ const int index,
const std::string &id, int win_width, int win_height)
{
if (index < 0 || index >= cloud.points.size ())
int hsize = fields[field_idx].count;
std::vector<double> array_x (hsize), array_y (hsize);
-
+
for (int i = 0; i < hsize; ++i)
{
array_x[i] = i;
memcpy (&data, reinterpret_cast<const char*> (&cloud.points[index]) + fields[field_idx].offset + i * sizeof (float), sizeof (float));
array_y[i] = data;
}
-
+
this->addPlotData(array_x, array_y, id.c_str(), vtkChart::LINE);
setWindowSize (win_width, win_height);
return (true);
}
+} // namespace visualization
+} // namespace pcl
+
#endif /* PCL_VISUALUALIZATION_PCL_PLOTTER_IMPL_H_ */
std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
j++;
+ ptr += 3;
}
nr_points = j;
points->SetNumberOfPoints (nr_points);
return (false);
vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
+ if (!polydata)
+ return false;
// Convert the PointCloud to VTK PolyData
convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
// Set the cells and the vertices
vertices->SetCells (nr_points, cells);
+ // Set the cell count explicitly as the array doesn't get modified enough so the above method updates accordingly. See #4001 and #3452
+ vertices->SetNumberOfCells(nr_points);
// Get the colors from the handler
bool has_colors = false;
*
*/
-#ifndef PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
-#define PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
+#pragma once
#include <set>
#include <map>
#include <pcl/pcl_macros.h>
#include <pcl/common/colors.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
+
+
+namespace pcl
+{
+
+namespace visualization
+{
-///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> vtkSmartPointer<vtkDataArray>
-
pcl::visualization::PointCloudColorHandlerCustom<PointT>::getColor () const
+PointCloudColorHandlerCustom<PointT>::getColor () const
{
if (!capable_ || !cloud_)
return nullptr;
return scalars;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> vtkSmartPointer<vtkDataArray>
-
pcl::visualization::PointCloudColorHandlerRandom<PointT>::getColor () const
+PointCloudColorHandlerRandom<PointT>::getColor () const
{
if (!capable_ || !cloud_)
return nullptr;
double r, g, b;
pcl::visualization::getRandomColors (r, g, b);
- int r_ = static_cast<int> (pcl_lrint (r * 255.0)),
- g_ = static_cast<int> (pcl_lrint (g * 255.0)),
+ int r_ = static_cast<int> (pcl_lrint (r * 255.0)),
+ g_ = static_cast<int> (pcl_lrint (g * 255.0)),
b_ = static_cast<int> (pcl_lrint (b * 255.0));
// Color every point
return scalars;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-
pcl::visualization::PointCloudColorHandlerRGBField<PointT>::setInputCloud (
+PointCloudColorHandlerRGBField<PointT>::setInputCloud (
const PointCloudConstPtr &cloud)
{
PointCloudColorHandler<PointT>::setInputCloud (cloud);
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> vtkSmartPointer<vtkDataArray>
-
pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor () const
+PointCloudColorHandlerRGBField<PointT>::getColor () const
{
if (!capable_ || !cloud_)
return nullptr;
{
// Copy the value at the specified field
if (!std::isfinite (cloud_->points[cp].x) ||
- !std::isfinite (cloud_->points[cp].y) ||
+ !std::isfinite (cloud_->points[cp].y) ||
!std::isfinite (cloud_->points[cp].z))
continue;
return scalars;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT>
-pcl::visualization::PointCloudColorHandlerHSVField<PointT>::PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud) :
-
pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
+PointCloudColorHandlerHSVField<PointT>::PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud) :
+ PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
{
// Check for the presence of the "H" field
field_idx_ = pcl::getFieldIndex<PointT> ("h", fields_);
capable_ = true;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> vtkSmartPointer<vtkDataArray>
-
pcl::visualization::PointCloudColorHandlerHSVField<PointT>::getColor () const
+PointCloudColorHandlerHSVField<PointT>::getColor () const
{
if (!capable_ || !cloud_)
return nullptr;
return scalars;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-
pcl::visualization::PointCloudColorHandlerGenericField<PointT>::setInputCloud (
+PointCloudColorHandlerGenericField<PointT>::setInputCloud (
const PointCloudConstPtr &cloud)
{
PointCloudColorHandler<PointT>::setInputCloud (cloud);
capable_ = false;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> vtkSmartPointer<vtkDataArray>
-
pcl::visualization::PointCloudColorHandlerGenericField<PointT>::getColor () const
+PointCloudColorHandlerGenericField<PointT>::getColor () const
{
if (!capable_ || !cloud_)
return nullptr;
return scalars;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-
pcl::visualization::PointCloudColorHandlerRGBAField<PointT>::setInputCloud (
+PointCloudColorHandlerRGBAField<PointT>::setInputCloud (
const PointCloudConstPtr &cloud)
{
PointCloudColorHandler<PointT>::setInputCloud (cloud);
capable_ = false;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> vtkSmartPointer<vtkDataArray>
-
pcl::visualization::PointCloudColorHandlerRGBAField<PointT>::getColor () const
+PointCloudColorHandlerRGBAField<PointT>::getColor () const
{
if (!capable_ || !cloud_)
return nullptr;
return scalars;
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> void
-
pcl::visualization::PointCloudColorHandlerLabelField<PointT>::setInputCloud (const PointCloudConstPtr &cloud)
+PointCloudColorHandlerLabelField<PointT>::setInputCloud (const PointCloudConstPtr &cloud)
{
PointCloudColorHandler<PointT>::setInputCloud (cloud);
field_idx_ = pcl::getFieldIndex<PointT> ("label", fields_);
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT> vtkSmartPointer<vtkDataArray>
-
pcl::visualization::PointCloudColorHandlerLabelField<PointT>::getColor () const
+PointCloudColorHandlerLabelField<PointT>::getColor () const
{
if (!capable_ || !cloud_)
return nullptr;
return scalars;
}
-#endif // PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
+} // namespace visualization
+} // namespace pcl
* $Id: point_cloud_handlers.hpp 7678 2012-10-22 20:54:04Z rusu $
*
*/
+
#ifndef PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_
#define PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_
#include <pcl/pcl_macros.h>
-///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
+namespace visualization
+{
+
template <typename PointT>
-pcl::visualization::PointCloudGeometryHandlerXYZ<PointT>::PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud)
- :
pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
+PointCloudGeometryHandlerXYZ<PointT>::PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud)
+ : PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
{
field_x_idx_ = pcl::getFieldIndex<PointT> ("x", fields_);
if (field_x_idx_ == -1)
capable_ = true;
}
-///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-
pcl::visualization::PointCloudGeometryHandlerXYZ<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
+
+template <typename PointT> void
+PointCloudGeometryHandlerXYZ<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
{
if (!capable_)
return;
}
}
-///////////////////////////////////////////////////////////////////////////////////////////
+
template <typename PointT>
-pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud)
- :
pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
+PointCloudGeometryHandlerSurfaceNormal<PointT>::PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud)
+ : PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
{
field_x_idx_ = pcl::getFieldIndex<PointT> ("normal_x", fields_);
if (field_x_idx_ == -1)
capable_ = true;
}
-///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-
pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
+
+template <typename PointT> void
+PointCloudGeometryHandlerSurfaceNormal<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
{
if (!capable_)
return;
}
}
+} // namespace visualization
+} // namespace pcl
+
#define PCL_INSTANTIATE_PointCloudGeometryHandlerXYZ(T) template class PCL_EXPORTS pcl::visualization::PointCloudGeometryHandlerXYZ<T>;
#define PCL_INSTANTIATE_PointCloudGeometryHandlerSurfaceNormal(T) template class PCL_EXPORTS pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<T>;
*
*/
+#pragma once
+
#include <thread>
-//////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+
template<typename PointSource, typename PointTarget> void
-
pcl::RegistrationVisualizer<PointSource, PointTarget>::startDisplay ()
+RegistrationVisualizer<PointSource, PointTarget>::startDisplay ()
{
// Create and start the rendering thread. This will open the display window.
viewer_thread_ = std::thread (&pcl::RegistrationVisualizer<PointSource, PointTarget>::runDisplay, this);
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> void
-
pcl::RegistrationVisualizer<PointSource, PointTarget>::stopDisplay ()
+RegistrationVisualizer<PointSource, PointTarget>::stopDisplay ()
{
// Stop the rendering thread. This will kill the display window.
viewer_thread_.~thread ();
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> void
-
pcl::RegistrationVisualizer<PointSource, PointTarget>::runDisplay ()
+RegistrationVisualizer<PointSource, PointTarget>::runDisplay ()
{
// Open 3D viewer
viewer_
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////
+
template<typename PointSource, typename PointTarget> void
-
pcl::RegistrationVisualizer<PointSource, PointTarget>::updateIntermediateCloud (
+RegistrationVisualizer<PointSource, PointTarget>::updateIntermediateCloud (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
// Unlock local buffers
visualizer_updating_mutex_.unlock ();
}
+
+} // namespace pcl
+
this->transform_->SetMatrix (t->GetMatrix());
}
- void applyInternals (vtkContext2D *painter)
+ void applyInternals (vtkContext2D *painter) const
{
painter->ApplyPen (pen_);
painter->ApplyBrush (brush_);
* \return[in] array containing the width and height of the window
*/
int *
- getWindowSize ();
+ getWindowSize () const;
/** \brief displays all the figures added in a window.
*/
* \return[in] array containing the width and height of the window
*/
int*
- getWindowSize ();
+ getWindowSize () const;
/** \brief Return a pointer to the underlying VTK RenderWindow used. */
vtkSmartPointer<vtkRenderWindow>
*/
PCLVisualizer (const std::string &name = "", const bool create_interactor = true);
- /** \brief PCL Visualizer constructor. It looks through the passed argv arguments to find the "-cam *.cam" argument.
+ /** \brief PCL Visualizer constructor. It looks through the passed argv arguments to find the "-cam *.cam" argument.
* If the search failed, the name for cam file is calculated with boost uuid. If there is no such file, camera is not initilalized.
* \param[in] argc
* \param[in] argv
*/
PCLVisualizer (int &argc, char **argv, const std::string &name = "",
PCLVisualizerInteractorStyle* style = PCLVisualizerInteractorStyle::New (), const bool create_interactor = true);
-
+
/** \brief PCL Visualizer constructor.
- * \param[in] custom vtk renderer
- * \param[in] custom vtk render window
+ * \param[in] ren custom vtk renderer
+ * \param[in] wind custom vtk render window
* \param[in] create_interactor if true (default), create an interactor, false otherwise
*/
PCLVisualizer (vtkSmartPointer<vtkRenderer> ren, vtkSmartPointer<vtkRenderWindow> wind, const std::string &name = "", const bool create_interactor = true);
/** \brief PCL Visualizer constructor.
* \param[in] argc
* \param[in] argv
- * \param[in] custom vtk renderer
- * \param[in] custom vtk render window
+ * \param[in] ren custom vtk renderer
+ * \param[in] wind custom vtk render window
* \param[in] style interactor style (defaults to PCLVisualizerInteractorStyle)
* \param[in] create_interactor if true (default), create an interactor, false otherwise
*/
spinOnce (int time = 1, bool force_redraw = false);
/** \brief Adds a widget which shows an interactive axes display for orientation
- * \param[in] interactor - Pointer to the vtk interactor object used by the PCLVisualizer window
+ * \param[in] interactor - Pointer to the vtk interactor object used by the PCLVisualizer window
*/
void
addOrientationMarkerWidgetAxes (vtkRenderWindowInteractor* interactor);
-
+
/** \brief Disables the Orientatation Marker Widget so it is removed from the renderer */
void
removeOrientationMarkerWidgetAxes ();
/** \brief Update a text to screen
* \param[in] text the text to update
* \param[in] xpos the new X position on screen
- * \param[in] ypos the new Y position on screen
+ * \param[in] ypos the new Y position on screen
* \param[in] id the text object id (default: equal to the "text" parameter)
*/
bool
/** \brief Update a text to screen
* \param[in] text the text to update
* \param[in] xpos the new X position on screen
- * \param[in] ypos the new Y position on screen
+ * \param[in] ypos the new Y position on screen
* \param[in] r the red color value
* \param[in] g the green color value
* \param[in] b the blue color value
* \param[in] id the text object id (default: equal to the "text" parameter)
*/
bool
- updateText (const std::string &text,
+ updateText (const std::string &text,
int xpos, int ypos, double r, double g, double b,
const std::string &id = "");
/** \brief Update a text to screen
* \param[in] text the text to update
* \param[in] xpos the new X position on screen
- * \param[in] ypos the new Y position on screen
+ * \param[in] ypos the new Y position on screen
* \param[in] fontsize the fontsize of the text
* \param[in] r the red color value
* \param[in] g the green color value
* \param[in] id the text object id (default: equal to the "text" parameter)
*/
bool
- updateText (const std::string &text,
+ updateText (const std::string &text,
int xpos, int ypos, int fontsize, double r, double g, double b,
const std::string &id = "");
- /** \brief Set the pose of an existing shape.
- *
- * Returns false if the shape doesn't exist, true if the pose was successfully
+ /** \brief Set the pose of an existing shape.
+ *
+ * Returns false if the shape doesn't exist, true if the pose was successfully
* updated.
*
* \param[in] id the shape or cloud object id (i.e., given on \a addLine etc.)
const typename pcl::PointCloud<pcl::PrincipalCurvatures>::ConstPtr &pcs,
int level = 100, float scale = 1.0f,
const std::string &id = "cloud", int viewport = 0);
-
+
/** \brief Add the estimated principal curvatures of a Point Cloud to screen.
* \param[in] cloud the input point cloud dataset containing the XYZ data
* \param[in] normals the input point cloud dataset containing the normal data
int viewport = 0)
{
// If Nth not given, display all correspondences
- return (addCorrespondences<PointT> (source_points, target_points,
+ return (addCorrespondences<PointT> (source_points, target_points,
correspondences, 1, id, viewport));
}
bool
setPointCloudRenderingProperties (int property, double val1, double val2,
const std::string &id = "cloud", int viewport = 0);
-
+
/** \brief Set the rendering properties of a PointCloud
* \param[in] property the property type
* \param[in] value the value to be set
bool
getPointCloudRenderingProperties (int property, double &value,
const std::string &id = "cloud");
-
+
/** \brief Get the rendering properties of a PointCloud
* \param[in] property the property type
* \param[out] val1 the resultant property value
getPointCloudRenderingProperties (RenderingProperties property, double &val1, double &val2, double &val3,
const std::string &id = "cloud");
- /** \brief Set whether the point cloud is selected or not
+ /** \brief Set whether the point cloud is selected or not
* \param[in] selected whether the cloud is selected or not (true = selected)
* \param[in] id the point cloud object id (default: cloud)
*/
bool
setPointCloudSelected (const bool selected, const std::string &id = "cloud" );
-
+
/** \brief Set the rendering properties of a shape
* \param[in] property the property type
* \param[in] value the value to be set
* \param[in] ymax the maximum Y coordinate for the viewport (0.0 <= 1.0)
* \param[in] viewport the id of the new viewport
*
- * \note If no renderer for the current window exists, one will be created, and
- * the viewport will be set to 0 ('all'). In case one or multiple renderers do
+ * \note If no renderer for the current window exists, one will be created, and
+ * the viewport will be set to 0 ('all'). In case one or multiple renderers do
* exist, the viewport ID will be set to the total number of renderers - 1.
*/
void
/** \brief Get camera file for camera parameter saving/restoring.
* \note This will be valid only when valid "-cam" option were available in command line
- * or a saved camera file were automatically loaded.
+ * or a saved camera file were automatically loaded.
* \sa cameraParamsSet (), cameraFileLoaded ()
*/
std::string
{
return (win_);
}
-
+
/** \brief Return a pointer to the underlying VTK Renderer Collection. */
vtkSmartPointer<vtkRendererCollection>
getRendererCollection ()
{
return (rens_);
}
-
+
/** \brief Return a pointer to the CloudActorMap this visualizer uses. */
CloudActorMapPtr
getCloudActorMap ()
{
return (cloud_actor_map_);
}
-
+
/** \brief Return a pointer to the ShapeActorMap this visualizer uses. */
ShapeActorMapPtr
getShapeActorMap ()
/** \brief Use Vertex Buffer Objects renderers.
* This is an optimization for the obsolete OpenGL backend. Modern OpenGL2 backend (VTK ≥ 6.3) uses vertex
* buffer objects by default, transparently for the user.
- * \param[in] use_vbos set to true to use VBOs
+ * \param[in] use_vbos set to true to use VBOs
*/
void
setUseVbos (bool use_vbos);
* attached to a given vtkRenderWindow
* \param[in,out] iren the vtkRenderWindowInteractor object to set up
* \param[in,out] win a vtkRenderWindow object that the interactor is attached to
- * \param[in,out] style a vtkInteractorStyle object
+ * \param[in,out] style a vtkInteractorStyle object
*/
void
setupInteractor (vtkRenderWindowInteractor *iren,
vtkRenderWindow *win,
vtkInteractorStyle *style);
-
+
/** \brief Get a pointer to the current interactor style used. */
inline vtkSmartPointer<PCLVisualizerInteractorStyle>
getInteractorStyle ()
{
return (new ExitMainLoopTimerCallback);
}
- void
+ void
Execute (vtkObject*, unsigned long event_id, void*) override;
int right_timer_id;
{
return (new ExitCallback);
}
- void
+ void
Execute (vtkObject*, unsigned long event_id, void*) override;
PCLVisualizer* pcl_visualizer;
FPSCallback (const FPSCallback& src) : vtkCommand (src), actor (src.actor), pcl_visualizer (src.pcl_visualizer), decimated (src.decimated), last_fps (src.last_fps) {}
FPSCallback& operator = (const FPSCallback& src) { actor = src.actor; pcl_visualizer = src.pcl_visualizer; decimated = src.decimated; last_fps = src.last_fps; return (*this); }
- void
+ void
Execute (vtkObject*, unsigned long event_id, void*) override;
vtkTextActor *actor;
/** \brief Internal pointer to widget which contains a set of axes */
vtkSmartPointer<vtkOrientationMarkerWidget> axes_widget_;
-
+
/** \brief Boolean that holds whether or not the camera parameters were manually initialized */
bool camera_set_;
* \param[in] actor a pointer to the vtk actor object
* \param[in] viewport port where the actor should be added to (default: 0/all)
*
- * \note If viewport is set to 0, the actor will be added to all existing
+ * \note If viewport is set to 0, the actor will be added to all existing
* renders. To select a specific viewport use an integer between 1 and N.
*/
void
void
createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data,
vtkSmartPointer<vtkActor> &actor,
- bool use_scalars = true);
+ bool use_scalars = true) const;
/** \brief Internal method. Creates a vtk actor from a vtk polydata object.
* \param[in] data the vtk polydata object to create an actor for
void
createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data,
vtkSmartPointer<vtkLODActor> &actor,
- bool use_scalars = true);
+ bool use_scalars = true) const;
/** \brief Converts a PCL templated PointCloud object to a vtk polydata object.
* \param[in] cloud the input PCL PointCloud dataset
* \param[in] tex_mat texture material in PCL format
* \param[out] vtk_tex texture material in VTK format
* \return 0 on success and -1 else.
- * \note for now only image based textures are supported, image file must be in
+ * \note for now only image based textures are supported, image file must be in
* tex_file attribute of \a tex_mat.
*/
int
*/
std::string
getUniqueCameraFile (int argc, char **argv);
-
+
//There's no reason these conversion functions shouldn't be public and static so others can use them.
public:
/** \brief Convert Eigen::Matrix4f to vtkMatrix4x4
convertToVtkMatrix (const Eigen::Vector4f &origin,
const Eigen::Quaternion<float> &orientation,
vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
-
+
/** \brief Convert vtkMatrix4x4 to an Eigen4f
* \param[in] vtk_matrix the original VTK 4x4 matrix
* \param[out] m the resultant Eigen 4x4 matrix
* This virtual method should not be overriden or used. The default implementation
* is provided only for backwards compatibility with handlers that were written
* before PCL 1.10.0 and will be removed in future. */
- PCL_DEPRECATED("use getColor() without parameters instead")
+ PCL_DEPRECATED(1, 12, "use getColor() without parameters instead")
virtual bool
getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
scalars = getColor ();
* This virtual method should not be overriden or used. The default implementation
* is provided only for backwards compatibility with handlers that were written
* before PCL 1.10.0 and will be removed in future. */
- PCL_DEPRECATED("use getColor() without parameters instead")
+ PCL_DEPRECATED(1, 12, "use getColor() without parameters instead")
virtual bool
getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
scalars = getColor ();
performSinglePick (vtkRenderWindowInteractor *iren, float &x, float &y, float &z);
int
- performAreaPick (vtkRenderWindowInteractor *iren, std::vector<int> &indices);
+ performAreaPick (vtkRenderWindowInteractor *iren, std::vector<int> &indices) const;
private:
float x_, y_, z_;
class vtkOpenGLExtensionManager;
class vtkRenderWindow;
+PCL_DEPRECATED(1, 13, "The OpenGL backend of VTK is deprecated. Please switch to the OpenGL2 backend.")
class PCL_EXPORTS vtkVertexBufferObject : public vtkObject
{
public:
#pragma once
#include <pcl/pcl_exports.h>
+#include <pcl/pcl_macros.h>
#include "vtkMapper.h"
#include "vtkSmartPointer.h"
class vtkShaderProgram2;
class vtkVertexBufferObject;
+PCL_DEPRECATED(1, 13, "The OpenGL backend of VTK is deprecated. Please switch to the OpenGL2 backend.")
class PCL_EXPORTS vtkVertexBufferObjectMapper : public vtkMapper
{
public:
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/boost.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
#include <mutex>
#include <thread>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/eigen.h>
-#include <pcl/make_shared.h>
+#include <pcl/memory.h>
//////////////////////////////////////////////////////////////////////////////////////////////
void
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int*
-pcl::visualization::PCLPainter2D::getWindowSize ()
+pcl::visualization::PCLPainter2D::getWindowSize () const
{
int *sz = new int[2];
sz[0] = win_width_;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int*
-pcl::visualization::PCLPlotter::getWindowSize ()
+pcl::visualization::PCLPlotter::getWindowSize () const
{
int *sz = new int[2];
sz[0] = win_width_;
void
pcl::visualization::PCLVisualizer::createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data,
vtkSmartPointer<vtkLODActor> &actor,
- bool use_scalars)
+ bool use_scalars) const
{
// If actor is not initialized, initialize it here
if (!actor)
void
pcl::visualization::PCLVisualizer::createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data,
vtkSmartPointer<vtkActor> &actor,
- bool use_scalars)
+ bool use_scalars) const
{
// If actor is not initialized, initialize it here
if (!actor)
}
/////////////////////////////////////////////////////////////////////////////////////////////
+PCL_DEPRECATED(1, 12, "This method can safely not be called anymore as we're just re-rendering all scenes now.")
void
pcl::visualization::PCLVisualizer::updateCamera ()
{
- PCL_WARN ("[pcl::visualization::PCLVisualizer::updateCamera()] This method was deprecated, just re-rendering all scenes now.");
rens_->InitTraversal ();
// Update the camera parameters
win_->Render ();
vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
data->SetNumberOfComponents (3);
+
vtkIdType nr_points = cloud_->width * cloud_->height;
+
+ if (!data->Resize(nr_points))
+ {
+ PCL_ERROR("[point_cloud_handlers::getGeometry] Failed to allocate space for points in VTK array.");
+ throw std::bad_alloc();
+ }
+
// Add all points
- float dim;
- vtkIdType j = 0; // true point index
- float* pts = static_cast<float*> (malloc (nr_points * 3 * sizeof (float)));
int point_offset = 0;
// If the dataset has no invalid values, just copy all of them
{
for (vtkIdType i = 0; i < nr_points; ++i, point_offset+=cloud_->point_step)
{
- // Copy the value at the specified field
- memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset], sizeof (float));
- pts[i * 3 + 0] = dim;
+ const float* ptr = reinterpret_cast<const float*>(&cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset]);
+ data->InsertNextValue(*ptr);
- memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset], sizeof (float));
- pts[i * 3 + 1] = dim;
+ ptr = reinterpret_cast<const float*>(&cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset]);
+ data->InsertNextValue(*ptr);
- memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset], sizeof (float));
- pts[i * 3 + 2] = dim;
+ ptr = reinterpret_cast<const float*>(&cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset]);
+ data->InsertNextValue(*ptr);
}
- data->SetArray (&pts[0], nr_points * 3, 0);
points->SetData (data);
}
else
{
for (vtkIdType i = 0; i < nr_points; ++i, point_offset+=cloud_->point_step)
{
- // Copy the value at the specified field
- memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset], sizeof (float));
- if (!std::isfinite (dim))
+ const float* ptr = reinterpret_cast<const float*>(&cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset]);
+ if (!std::isfinite (*ptr))
continue;
- pts[j * 3 + 0] = dim;
+ data->InsertNextValue(*ptr);
- memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset], sizeof (float));
- if (!std::isfinite (dim))
+ ptr = reinterpret_cast<const float*>(&cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset]);
+ if (!std::isfinite (*ptr))
continue;
- pts[j * 3 + 1] = dim;
+ data->InsertNextValue(*ptr);
- memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset], sizeof (float));
- if (!std::isfinite (dim))
+ ptr = reinterpret_cast<const float*>(&cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset]);
+ if (!std::isfinite (*ptr))
continue;
- pts[j * 3 + 2] = dim;
-
- // Set j and increment
- j++;
+ data->InsertNextValue(*ptr);
}
- data->SetArray (&pts[0], j * 3, 0);
points->SetData (data);
}
}
/////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::visualization::PointPickingCallback::performAreaPick (vtkRenderWindowInteractor *iren,
- std::vector<int> &indices)
+ std::vector<int> &indices) const
{
vtkAreaPicker *picker = static_cast<vtkAreaPicker*> (iren->GetPicker ());
vtkRenderer *ren = iren->FindPokedRenderer (iren->GetEventPosition ()[0], iren->GetEventPosition ()[1]);
projects / pcl.git / commitdiff
![doc/tutorials/content/images/form_0.png](/"http://www.example.com/cgi/"?p=pcl.git;a=blob;f=doc/tutorials/content/images/form_0.png;h=8312220ee8737506e28cdb76500f7bd400b06d99;hb=d0a7ef075ae569d3355d4b5fe9b7d9b256ea2fe7){kind=link}
![history](/"http://www.example.com/cgi/"?p=pcl.git;a=history;f=doc/tutorials/content/images/form_0.png;hb=d0a7ef075ae569d3355d4b5fe9b7d9b256ea2fe7){kind=link}
![doc/tutorials/content/images/form_1.png](/"http://www.example.com/cgi/"?p=pcl.git;a=blob;f=doc/tutorials/content/images/form_1.png;h=fb127a641afee88f09151c1580f49e4b3010860b;hb=d0a7ef075ae569d3355d4b5fe9b7d9b256ea2fe7){kind=link}
![history](/"http://www.example.com/cgi/"?p=pcl.git;a=history;f=doc/tutorials/content/images/form_1.png;hb=d0a7ef075ae569d3355d4b5fe9b7d9b256ea2fe7){kind=link}
![a/doc/tutorials/content/images/form_0.png](/"http://www.example.com/cgi/"?p=pcl.git;a=blob;f=doc/tutorials/content/images/form_0.png;h=8312220ee8737506e28cdb76500f7bd400b06d99){kind=link}
![a/doc/tutorials/content/images/form_1.png](/"http://www.example.com/cgi/"?p=pcl.git;a=blob;f=doc/tutorials/content/images/form_1.png;h=fb127a641afee88f09151c1580f49e4b3010860b){kind=link}